基于S 型加減速的六關(guān)節(jié)機器人速度平滑控制算法

韓媞

（大連東軟信息學(xué)院智能與電子工程學(xué)院，遼寧大連 116023）

工業(yè)機器人在運動過程中需要聯(lián)合控制多個軸的運動，并且會根據(jù)實時數(shù)據(jù)進(jìn)行動態(tài)調(diào)整。為了獲得良好的運動性能和精度，需要機器人運動軌跡具備高度平滑性、系統(tǒng)加減速具有高度柔性[1-3]。

六關(guān)節(jié)機器人在工業(yè)中應(yīng)用廣泛，經(jīng)常用于拾取、搬運、焊接等場合。機器人有示教或離線編程兩種工作方式。一般情況下，路徑是通過給定若干個位置點來確定的[4-7]。那么，機器人在相鄰兩個軌跡的拐點處的速度處理就決定了機器人的運行速度和運行平穩(wěn)性[8-11]，是影響機器人性能的重要因素。

目前，機器人運動系統(tǒng)中常用的加減速控制算法有指數(shù)、梯形、S 型曲線和三角函數(shù)等[12-14]。目前的設(shè)備中，在一些要求不高的場合，一般使用梯形加減速或指數(shù)型加減速控制，算法原理簡單，但是在設(shè)備的啟停階段，其加速度不為零，所以會出現(xiàn)速度和加速度突變的情況，對大型設(shè)備沖擊較明顯，尤其不適用于工業(yè)機器人等高速運動系統(tǒng)的過程控制。由于S 型曲線加減速方法的速度和加速度變化連續(xù)，原理簡單，易于實現(xiàn)，并且能夠得出平滑的速度和加速度曲線，所以應(yīng)用較廣泛。文中通過使用S 型加減速原理來處理機器人在PTP（Point to Point）工作模式下拐點處的速度平滑過渡，實現(xiàn)了機器人快速、平穩(wěn)、高精度的運動控制。

1 數(shù)學(xué)原理

1.1 S型加減速

S 型加減速是加減速控制中一個比較常用的方法[15-16]，該方法通過控制加加速度得到加速度曲線為梯形或者三角形，速度曲線為“S”型，從而使速度曲線變化柔和，控制加加速度值減小沖擊，其加速度曲線如圖1 所示，分為3 個階段，加加速度階段、勻加速階段、減加速階段，對應(yīng)的速度曲線如圖2所示。

圖1 加速度曲線圖

圖2 S型速度曲線圖

通過約束最大加加速度JMAX值，來控制加減速過程中產(chǎn)生的沖擊，從而得到S 型速度曲線，加速度公式如下所示：

由微積分原理可以得到速度公式如式（2）所示，其中，V0為初始速度。

由積分原理可得距離公式如式（3）所示，其中，V0為初始速度。

1.2 過渡算法

在拐角前后的路徑中，六關(guān)節(jié)機器人各個物理軸的速度可以分別設(shè)定為，其中上標(biāo)表示第幾個軸，下標(biāo)表示第幾段軌跡的速度。各個軸的速度如圖3 所示。

圖3 物理軸速度曲線

從圖3 中可以看到，不同軌跡段中各個物理軸的目標(biāo)速度是不同的。拐角速度平滑處理的主要思路就是在保證誤差的基礎(chǔ)上，每個物理軸各自計算在拐角處從前一段的V1N到下一段的V2N的平滑過渡過程。

速度控制的要求是每個物理軸的速度變化要平順，不能超過各自給定的最大加速度和加加速度的約束。在拐角處對各物理軸分別進(jìn)行S 型加減速就可以達(dá)到目的，如圖4 所示。

圖4 單個物理軸的速度過渡示意圖

過渡算法不僅要保證加速度和加加速度不超過給定約束，還需要保證機器人各物理軸在運動過程中的誤差在給定的約束范圍內(nèi)，從而確保機器人端部位置在誤差約束范圍內(nèi)[8]。

通過給定各個軸的最大誤差約束和最大加速度以及加加速度約束，保證所有物理軸的過渡時間保持一致。通過計算各個物理軸的速度差和約束條件，計算出每個物理軸需要的過渡時間，取最大的過渡時間作為統(tǒng)一過渡時間，再根據(jù)該時間修正每個物理軸在過渡過程中所需要的臨時最大加速度和加加速度，作為當(dāng)前過渡計算的條件值，最終完成過渡過程的計算。

1.3 具體計算

假設(shè)各軸的最大誤差為δ，一般情況下各個軸可以設(shè)置成一樣的誤差值，設(shè)成不同的誤差值也可以，不影響后續(xù)的計算。為簡單起見，這里設(shè)置成相同的誤差值。

假設(shè)各個物理軸的加速度約束為an，其中n表示第n個軸，Jn為各個軸的加加速度約束。

根據(jù)各個物理軸兩段軌跡的速度差值、最大加速度值和加加速度約束值可以得到一個過渡時間Tn。

為了保證軌跡的正確性，各個物理軸需要保證時序上的統(tǒng)一，因此需要確定最終統(tǒng)一的過渡時間TF=MAX(T1,T2,T3,T4,T5,T6)。完成統(tǒng)一過渡時間的計算后，各個物理軸再根據(jù)時間計算當(dāng)前過渡段所需要的加加速度值和最大加速度值。

單個物理軸的誤差值Sδ的計算公式如式（4）所示：

過渡部分最大誤差示意圖如圖5 所示。

圖5 過渡部分最大誤差示意圖

可見Sδ為偏離原軌跡的距離，原軌跡為單個物理軸通過程序指定的軌跡，其速度曲線是確定的，當(dāng)速度偏離原速度曲線后，產(chǎn)生的偏離距離即為誤差。

由于在過渡過程中S 型加減速在拐點兩側(cè)的速度與原始速度曲線在時間上是對稱的，因此最終過渡后單個物理軸的誤差會消除掉，機器人最終回到了原始軌跡上。

T0到T1之間是過渡時間，在這段時間內(nèi)非S 型過渡的距離是S1，使用S 型過渡的距離是S2，如圖6 所示，這兩段距離是相等的，單個物理軸的誤差值是可以忽略的。

圖6 過渡部分整體誤差示意圖

2 實驗驗證

實驗條件：有兩段機器人運動的軌跡以及進(jìn)給速度，使用C++開發(fā)環(huán)境VS2013 進(jìn)行編程，根據(jù)各個物理軸第一段和第二段的速度，按照插補周期為2 ms 計算出過渡過程中的插補數(shù)據(jù)。

根據(jù)插補數(shù)據(jù)畫出各個物理軸的速度曲線，來證明在過渡過程中各個物理軸的速度、加速度以及誤差值都不超過限定值，從而證明該算法的正確性。

在機器人隨機運動過程中，隨機選取拐點的3 個軌跡點的六軸空間內(nèi)的坐標(biāo)如下所示：

其中，Ai代表機器人的第i個軸坐標(biāo)。

進(jìn)給速度F=10 000 mm/min，按照PTP 模式第一點到第二點的軌跡段中，各個物理軸的速度由公式計算得到：

設(shè)6 個物理軸的最大加速度約束值為100 °/s2，最大加加速度約束值為1 000 °/s3。計算可得過渡時間為0.2 s。

6 個物理軸在兩段之間速度變化最大值為136.082 °/s，根據(jù)加加速度約束值，可以得到一個加速度三角形，如圖7 所示。

圖7 實際過程的加速度示意圖

加速度三角形斜率為加加速度限制值，三角形面積為速度差值，由此可以計算得到AMAX=3.687 8 °/s2，T0=0.036 878 s。

A1～A6的過渡速度曲線如圖4 所示。由此進(jìn)行速度插補，得到各個物理軸的實際插補速度如圖8 所示，實際插補加速度如圖9 所示，圖中由上至下分別為A1～A6軸的速度和加速度。

圖8 實際插補數(shù)據(jù)速度示意圖

圖9 實際插補數(shù)據(jù)加速度示意圖

過渡過程中各個物理軸的最終誤差均為0，最大誤差如下所示：

ERROR1=0.000 000 00，ERROR2=0.000 264 336，ERROR3=3.887 3×10-5，ERROR4=0.000 264 336，ERR OR5=0.000 186 59，ERROR6=0.000 225 463。

實際驗證的最終誤差符合理論分析結(jié)果，最大誤差控制在10-4范圍內(nèi)，滿足了機器人實際運行的精度誤差約束要求。

3 結(jié)論

在工業(yè)機器人運動控制系統(tǒng)中，需要6 個軸的位置在拐點處的速度控制穩(wěn)定，系統(tǒng)加速度變化較小。直線型、梯形或指數(shù)型加減速等算法在運動過程中會出現(xiàn)速度和加速度的突變，不適于大型設(shè)備如工業(yè)機器人等系統(tǒng)。為了改善六關(guān)節(jié)機器人的動力學(xué)性能，文中提出了S 型加減速的控制算法并使用實際的機器人運動數(shù)據(jù)進(jìn)行了C 程序的驗證。最終根據(jù)實際插補點的數(shù)據(jù)計算各個物理軸的速度、加速度、加加速度和最大誤差，證明了使用S 型速度變化曲線作為控制機器人各個物理軸的速度變化規(guī)律，既可以滿足各物理軸的加速度和加加速度的極限值約束，也可以滿足機器人軌跡的誤差約束，實現(xiàn)了對機器人良好、平滑的速度控制。