面向工業(yè)機器人的位姿同步加減速算法

陳多多，李麗宏，原 鋼

（1.太原理工大學(xué)電氣與動力工程學(xué)院，太原 030024；2.中國煤炭科工集團太原研究院，太原 030006）

對工業(yè)機器人來說，最常見的操作要求之一是在笛卡爾空間中將其末端執(zhí)行器從初始位姿按照指定的空間路徑平滑地移動至目標(biāo)位姿［1-3］。為實現(xiàn)上述過程，需進行位姿軌跡規(guī)劃［4］，即計算出空間路徑上每個點的位置坐標(biāo)（位置軌跡規(guī)劃），以及機器人在末端執(zhí)行器位于這些點時的姿態(tài)（姿態(tài)軌跡規(guī)劃），且要求位置軌跡與姿態(tài)軌跡光滑連續(xù)且同步運行［5-9］。

李振娜等［10］提出帶約束S型曲線加減速控制方法對位置軌跡、姿態(tài)軌跡進行規(guī)劃，根據(jù)總位移與加速段位移之間的關(guān)系等約束條件將S型速度曲線分為4種類型，算法實現(xiàn)較為復(fù)雜［11-14］，曲線首末速度為零，影響工業(yè)機器人作業(yè)執(zhí)行效率［15］。李振娜等［10］雖然給出位置軌跡和姿態(tài)軌跡的規(guī)劃過程，但未具體給出位姿同步規(guī)劃的處理過程。王斌銳等［16-17］采用非對稱S曲線加減速控制方法進行軌跡規(guī)劃，該方法相較于帶約束S型曲線加減速控制方法，曲線首末速度非必須為零，可實現(xiàn)工業(yè)機器人任務(wù)空間內(nèi)圓弧段路徑與直線段路徑的銜接，提高工業(yè)機器人作業(yè)執(zhí)行效率，但需考慮由于規(guī)劃運行速度較大或運行路徑較短等因素引起的勻加速段、勻速段或勻減速段缺失的情形，方法實現(xiàn)復(fù)雜。王斌銳等［16］同樣只給出位置軌跡和姿態(tài)軌跡的規(guī)劃過程，未具體給出位姿同步規(guī)劃的實現(xiàn)過程，而羅鈞等［17］提出的加減速控制方法對工業(yè)機器人軌跡規(guī)劃具有一定的參考意義，但文獻中主要將加減速控制方法集成于機床NC系統(tǒng)中，未提及機器人位姿同步規(guī)劃的方案。黃忠明等［18］采用正弦波加速度曲線加減速控制方法分別規(guī)劃位置軌跡和姿態(tài)軌跡，按照等時原則對2種軌跡進行了同步規(guī)劃處理。正弦波加速度曲線加減速控制方法加速度-時間（a-t）曲線按正弦規(guī)律變化，表達式簡單，加速度無階躍變化，但考慮到機器人運動過程中需要實時地更新路徑位置，而計算機計算三角函數(shù)的計算量較大，因此對機器人控制系統(tǒng)的實時性影響較大［19］，且等時原則同步規(guī)劃處理方法只是實現(xiàn)了位置軌跡和姿態(tài)軌跡的等時處理，未實現(xiàn)同步對象同步加速、同步勻速、同步減速的效果。

本文提出一種面向工業(yè)機器人的位姿同步加減速算法。本算法首先從正弦波加速度曲線加減速控制方法出發(fā)，提出一種基于函數(shù)逼近的四次多項式加減速控制方法（quartic polynomial acceleration and deceleration control method based on function approximation，QPADFA），接著在 QPADFA方法的基礎(chǔ)上采取完全同步控制策略給出機器人末端位置軌跡和姿態(tài)軌跡同步規(guī)劃處理的方法。

較S型曲線加減速控制方法，QPADFA方法的控制過程簡單，只需考慮初次規(guī)劃得到的加速段位移Sa與減速段位移Sd之和大于給定的總位移ST一種情況下的再次規(guī)劃問題，不必像S型曲線加減速控制方法那樣考慮無勻加（減）速段、無勻速段或兩者皆無的情況，易于編程實現(xiàn)。較正弦曲線加減速控制方法，QPADFA方法具有程序運行時間短、程序運行所占內(nèi)存小的優(yōu)勢。較等時原則，完全同步控制策略可實現(xiàn)工業(yè)機器人末端執(zhí)行器位置軌跡和姿態(tài)軌跡同步加速、同步勻速、同步減速的效果。

1 位姿同步加減速算法

1.1 QPADFA方法

本方法首先按照給定的勻速段運行速度、加速段最大加速度、減速段最大減速度和軌跡規(guī)劃總位移進行初次規(guī)劃，如果規(guī)劃得到的加速階段位移Sa與減速階段位移Sd之和小于等于總位移ST，則軌跡規(guī)劃完成，若不滿足該條件，則按照S′a+S′d＝ST原則進行再次規(guī)劃，S′a、S′d分別為再次規(guī)劃的加速階段位移、減速階段位移。

1.1.1 按給定信息進行初次規(guī)劃

正弦波加速度曲線加減速控制方法的加速度 -時間表達式［20］如下：

式中：ama、amd為給定參數(shù)，分別代表加速段最大加速度、減速段最大減速度；Ta、Td、T為待求參數(shù)，代表加速段持續(xù)時間、減速段持續(xù)時間及軌跡規(guī)劃總時間，其中 t1＝Ta，t2＝T-Td。

使用多項式對式（1）中的加速度函數(shù)進行最佳平方逼近。將式（1）中加速段表達式正弦函數(shù)部分簡單表示為 f（x）＝sin（π（x）），其中，x＝（t／Ta）∈［0，1］。使用線性無關(guān)函數(shù)族 φ0（x）＝1，φ1（x）＝x，φ2（x）＝x2進行最佳平方逼近，與函數(shù)族對應(yīng)的正規(guī)方程組為

其中，（φ0，φ0）表示兩函數(shù) φ0（x）與 φ0（x）在區(qū)間［0，1］上的內(nèi)積，其余表達式以此類推。由式（2）可解得 a0＝-0.05，a1＝4.122，a2＝-4.122，則所求最佳平方逼近多項式為

考慮到應(yīng)使變速段能夠達到給定最大加速度且變速段起始和終止時刻的加速度為零，所以將式（3）近似為下式：

同理，可求得減速段表達式最佳平方逼近多項式。則式（1）化簡為：

式中，t′＝t-t2。

由式（5）及邊界條件積分得速度-時間表達式：

式中：v0為給定的加速段起始速度；vm為給定的勻速段運行速度。

由式（6）及給定的減速段終止速度v2得加速段、減速段持續(xù)時間：

由式（6）（7）及邊界條件積分得位移-時間表達式：

式中：Sa為加速階段位移；Sc為勻速階段位移。

1.1.2 初次規(guī)劃失敗后的再次規(guī)劃

當(dāng)按照給定的ama、amd、vm規(guī)劃得到的加速段位移Sa與減速段位移Sd之和大于給定的總位移ST，即Sa+Sd＞ST時，上述規(guī)劃中無勻速段，此時需進行再次規(guī)劃。本方法重新規(guī)劃的原則是：取消勻速段，先從起始速度v0加速到一個盡可能大的中間速度vt（vt≤vm），然后減速到減速段終止速度v2，使再次規(guī)劃得到的加速段位移S′a與減速段位移S′d之和等于軌跡規(guī)劃總位移ST，即：

由式（8）可得再次規(guī)劃的位移-時間表達式為

式中：t′＝t-t1，S′a為 t＝t1時的位移值，即再次規(guī)劃得到的加速段位移。中間速度vt由以下操作求得。

由式（7）可知，加速段持續(xù)時間與減速段持續(xù)時間均與中間速度有關(guān)，這里的中間速度是指加速段完成時的臨界點速度，也就是減速段開始時的臨界點速度。因此，當(dāng)加速段與減速段之間的中間速度為vt時，有：

將式（11）代入式（10）可求得再次規(guī)劃后的加速段位移 S′a減速段位移 S′d，再由式（9）可得：

QPADFA方法的規(guī)劃流程如圖1所示。

圖1 QPADFA方法的規(guī)劃流程框圖

1.2 位姿同步規(guī)劃處理方法

對機器人的笛卡爾運動而言，其末端執(zhí)行器的位置軌跡規(guī)劃和姿態(tài)軌跡規(guī)劃需要同步進行。本文中，位置和姿態(tài)同步變化采取完全同步的控制策略，即首先按照給定位置軌跡參數(shù)和姿態(tài)軌跡參數(shù)分別對位置、姿態(tài)按前一節(jié)所述QPADFA方法進行軌跡規(guī)劃，得到位置軌跡的加速段持續(xù)時間Taw和姿態(tài)軌跡的加速段持續(xù)時間Taz，比較兩個加速段時間，選擇時間較長的作為位姿同步規(guī)劃中的加速段時間TaF。同樣，得到位置軌跡的減速段持續(xù)時間Tdw和姿態(tài)軌跡的減速段持續(xù)時間Tdz，比較2個減速段時間，選擇時間較長的作為位姿同步規(guī)劃中的減速段時間TdF，如果位置軌跡和姿態(tài)軌跡中至少有一個存在勻速段，則按照同樣的處理方法得到位姿同步規(guī)劃中的勻速段時間TcF。

在確定了軌跡規(guī)劃曲線各階段的時間后，軌跡各變速段的加速度不再確定，此時需重新討論確定新的位置軌跡和姿態(tài)軌跡的位移-時間表達式。

將QPADFA方法的加速段位置-時間、速度-時間、加速度-時間表達式寫成多項式形式如下：

若給定加速時間 Ta，并已知 a（0）＝0、a（Ta）＝0、v（0）＝v0，設(shè)加速段完成時的臨界點速度 v（Ta）＝vL。此時，式（13）中的加速段表達式各項系數(shù)為：

將t＝Ta代入式（13）求得加速段位移為：

同理，可求得減速段位移：

根據(jù)加速段位移Sa、勻速段位移Sc、減速段位移Sd之和等于總位移ST的原則，可求得

以上式子對再次規(guī)劃的情況也是適用的，此時Tc＝0。

將式（17）代入式（14）可得加速段持續(xù)時間確定下的加速段位移-時間表達式，同理可得減速段持續(xù)時間確定下的減速段位移-時間表達式，最后得出位姿同步變化的位移-時間表達式如下：

其中 t1＝TaF，t2-t1＝TcF，T-t2＝TdF，t′＝t-t2。

2 仿真驗證

2.1 多路徑過渡實驗

本實驗進行典型多路徑銜接模型的軌跡規(guī)劃。實驗?zāi)康臑轵炞CQPADFA方法根據(jù)路徑約束條件進行初次規(guī)劃或再次規(guī)劃的功能，主要參考文獻［16］給出的方法建立典型多路徑銜接模型。

實驗選取的典型多路徑由目標(biāo)點P0-P2組成。設(shè)定軌跡規(guī)劃勻速段運行速度為80 mm／s，最大加速度100 mm／s2，圓弧過渡段速度為20 mm／s。給定路徑信息如表1所示。

表1 路徑信息

路徑中，路徑點P0為直線路徑起始點，其速度為0 mm／s，半徑調(diào)節(jié)參數(shù)為0；路徑點P1為規(guī)劃路徑P0P1與P1P2的交點，因為直線路徑之間采用圓弧路徑過渡的方式進行，其速度不應(yīng)為零，且小于等于給定勻速段運行速度，定義其為20 mm／s，定義半徑調(diào)節(jié)參數(shù)為0.1，求得過渡圓弧路徑的半徑為56.5 mm；路徑點P2為直線路徑終止點，其速度為0 mm／s，半徑調(diào)節(jié)參數(shù)為0。

按照圖1給出的QPADFA方法規(guī)劃流程圖進行軌跡規(guī)劃，得到的機器人末端軌跡、末端速度和加速度曲線分別如圖2、3所示。圖2中，Pi為機器人任務(wù)空間中的目標(biāo)點，PGi為路徑過渡銜接點，C1為過渡圓弧路徑的圓心。根據(jù)表1給出的路徑參數(shù)及文獻［16］提供的計算方法可得：路徑一P0PG1的總位移ST1＝59.40 mm，過渡圓弧路徑PG1PG2的總位移ST2＝88.86 mm，路徑二PG2P2的總位移ST3＝424.26 mm。

圖2 機器人末端軌跡

圖3 機器人末端速度及加速度曲線

由圖3可以看出：路徑一P0PG1初次規(guī)劃失敗，進行了再次規(guī)劃，再次規(guī)劃得到的最大運行速度為45.3 mm／s，加速階段最大加速度為56.6 mm／s2，減速階段最大減速度為 -42.2 mm／s2，且由于路徑初始速度vp0與終止速度vpG1不一致，所以其速度曲線和加速度曲線是非對稱的。機器人末端以給定速度20 mm／s勻速通過過渡圓弧路徑PG1PG2。路徑二PG2P2初次規(guī)劃成功，加速階段最大加速度達到了給定的100 mm／s2，加速階段結(jié)束時機器人末端速度為給定的勻速運行速度80 mm／s，之后按該速度勻速運行，勻速段結(jié)束后，進入減速階段，減速階段最大減速度為-100 mm／s2，速度逐漸降至 0 mm／s。

由多路徑過渡實驗可知：本文軌跡規(guī)劃方法QPADFA實現(xiàn)了速度的平滑過渡，并可在路徑約束條件下進行速度和加速度的自行調(diào)整，驗證了QPADFA方法的有效性。

2.2 加減速控制方法效果對比實驗

本實驗從規(guī)劃運動時間、程序運行時間及對運動曲線平滑性的影響3個方面對QPADFA方法、非對稱S曲線加減速控制方法、帶約束S型曲線加減速控制方法及正弦波加速度曲線加減速控制方法進行對比。給定直線路徑P0P1信息如表2所示。路徑中，路徑點P0為直線路徑起始點，其速度應(yīng)為0 mm／s，半徑調(diào)節(jié)參數(shù)應(yīng)為0；路徑點P1為直線路徑終止點，其速度應(yīng)為0 mm／s，半徑調(diào)節(jié)參數(shù)應(yīng)為0。

表2 路徑信息

由表2得位置總位移ST＝1 000 mm。給定加速段起始速度v0＝0 mm／s，勻速段運行速度vm＝500 mm／s，加速段最大加速度 ama＝1 000 mm／s2、減速段最大減速度amd＝-1 000 mm／s2，減速段終止速度 v2＝0 mm／s。

通過Matlab仿真，得到4種方法的程序運行時間及規(guī)劃運動時間，如表3所示，QPADFA方法與非對稱S曲線加減速控制方法的加速度曲線如圖4、QPADFA方法與帶約束S型曲線加減速控制方法的加速度曲線如圖5、QPADFA方法與正弦波加速度曲線加減速控制方法的加速度曲線如圖6。

表3 4種方法的程序運行時間及規(guī)劃運動時間 ms

圖4 QPADFA方法與非對稱S曲線加減速控制方法的加速度曲線

圖5 QPADFA方法與帶約束S型曲線加減速控制方法的加速度曲線

圖6 QPADFA方法與正弦波加速度曲線加減速控制方法的加速度曲線

由圖4、表3可以看出：相對于非對稱S曲線加減速控制方法，本文提出的QPADFA方法加速度曲線更加平滑，規(guī)劃運動時間縮短5%，程序運行時間縮短48.9%。

由圖5、表3可以看出：相對于帶約束S型曲線加減速控制方法，本文提出的QPADFA方法加速度曲線更加平滑，規(guī)劃運動時間縮短5%，程序運行時間縮短50.5%。

由圖6、表3可以看出：雖然本文提出的QPADFA方法的加速度曲線和正弦波加速度曲線加減速控制方法接近，但規(guī)劃運動時間縮短3.2%，程序運行時間縮短16.4%，在機器人實時控制系統(tǒng)中，更具實時性優(yōu)勢。

2.3 位姿同步規(guī)劃實驗

本實驗?zāi)康臑轵炞C本文提出的位姿同步加減速算法可以實現(xiàn)工業(yè)機器人末端執(zhí)行器位置和姿態(tài)的同步規(guī)劃。

設(shè)定位置軌跡規(guī)劃勻速段運行速度為80 mm／s，最大加速度 85 mm／s2；姿態(tài)軌跡規(guī)劃的勻速段運行速度為0.15 rad／s，最大姿態(tài)角加速度0.15 rad／s2；實驗起始點位姿矩陣AS和目標(biāo)點位姿矩陣AE分別為：

矩陣AS、AE分別為本課題研究使用的6關(guān)節(jié)型機器人處于2種不同狀態(tài)時的末端位姿矩陣。由矩陣AS、AE可得起始點與目標(biāo)點之間的位置偏移為423.73 mm，姿態(tài)對應(yīng)的四元數(shù)之間的夾角為0.82 rad。對位置和姿態(tài)按照QPADFA方法進行軌跡規(guī)劃，分別得到2種規(guī)劃的加速段時間Ta、勻速段時間Tc、減速段時間Td如表4所示。

表4 軌跡規(guī)劃時間

由表4可以看出：位置軌跡和姿態(tài)軌跡單獨規(guī)劃時加速段時間、勻速段時間、減速段時間是不相同的，按照本文給出的完全同步控制策略，確定位姿同步規(guī)劃的加速段時間TaF＝1.500 s，勻速段時間 TcF＝3.962 s，減速段時間 TdF＝1.500 s。確定各階段時間后，按照1.2節(jié)所述位姿同步規(guī)劃處理方法對位置軌跡和姿態(tài)軌跡進行同步規(guī)劃處理。

經(jīng)本文的位姿同步加減速算法得到的位置軌跡的位移-時間、速度-時間及加速度-時間曲線如圖7所示，姿態(tài)軌跡的角位移-時間、角速度-時間及角加速度-時間曲線如圖8所示。

圖7 位置軌跡規(guī)劃-時間曲線

圖8 姿態(tài)軌跡規(guī)劃-時間曲線

由圖7、8可知：位置軌跡規(guī)劃運動時間Tw＝6.962 s，姿態(tài)軌跡規(guī)劃運動時間 Tz＝6.962 s，兩種軌跡的加速段時間、勻速段時間、減速段時間均相同，該算法實現(xiàn)了位置軌跡和姿態(tài)軌跡的同步運行。由圖7可知：位置軌跡規(guī)劃中的勻速段速度不是設(shè)定的80 mm／s，而是 77.58 mm／s，這是進行位姿同步規(guī)劃處理的結(jié)果。

圖7、8得到的機器人末端執(zhí)行器位置軌跡規(guī)劃和姿態(tài)軌跡規(guī)劃的位移-時間曲線、速度-時間曲線是光滑連續(xù)的，因此可以忽略工業(yè)機器人在運動過程中的剛性沖擊；加速度-時間曲線無階躍且光滑連續(xù)，因此可以忽略工業(yè)機器人在運動過程中的柔性沖擊。本文提出的面向工業(yè)機器人的位姿同步加減速算法具有工程實用性。

3 結(jié)論

本文中提出了一種面向工業(yè)機器人的位姿同步加減速算法，包括QPADFA方法和位姿同步規(guī)劃處理方法2部分。實驗結(jié)果表明：相對于非對稱S曲線加減速控制方法、帶約束S型曲線加減速控制方法及正弦波加速度曲線加減速控制方法，QPADFA方法在規(guī)劃運動時間、時間復(fù)雜度、加速度曲線平滑性等方面有一定優(yōu)勢；位姿同步規(guī)劃處理方法實現(xiàn)了工業(yè)機器人末端執(zhí)行器位置軌跡和姿態(tài)軌跡同步加速、同步勻速、同步減速的效果，具有工程實用價值。本文提出的位姿同步加減速算法是將機器人末端位置和姿態(tài)單獨規(guī)劃后進行同步處理的，為了進一步提高機器人軌跡規(guī)劃的效率，下一步的工作為將位置和姿態(tài)統(tǒng)一規(guī)劃插補以實現(xiàn)綜合運算。