陳多多，李麗宏，原 鋼

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

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

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

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

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

1 位姿同步加減速算法

1.1 QPADFA方法

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

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

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

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

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

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

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

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

式中，t′＝t-t2。

由式（5）及邊界條件積分得速度-時(shí)間表達(dá)式：

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

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

由式（6）（7）及邊界條件積分得位移-時(shí)間表達(dá)式：

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

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

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

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

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

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

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

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

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

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

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

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

將QPADFA方法的加速段位置-時(shí)間、速度-時(shí)間、加速度-時(shí)間表達(dá)式寫成多項(xiàng)式形式如下：

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

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

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

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

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

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

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

2 仿真驗(yàn)證

2.1 多路徑過渡實(shí)驗(yàn)

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

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

表1 路徑信息

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

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

圖2 機(jī)器人末端軌跡

圖3 機(jī)器人末端速度及加速度曲線

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

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

2.2 加減速控制方法效果對(duì)比實(shí)驗(yàn)

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

表2 路徑信息

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

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

表3 4種方法的程序運(yùn)行時(shí)間及規(guī)劃運(yùn)動(dòng)時(shí)間 ms

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

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

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

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

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

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

2.3 位姿同步規(guī)劃實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

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

3 結(jié)論

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