一種串聯(lián)機器人笛卡爾空間軌跡規(guī)劃方法

郭 領

(南京航空航天大學機電學院，江蘇 南京 210016)

對于工業(yè)機器人而言，提高工作效率是一個重要目標。在機器人末端執(zhí)行器沿著固定路徑重復執(zhí)行工作任務的情況下，如何在避免沖擊且滿足驅動器性能要求的前提下盡可能減少單次任務的執(zhí)行時間是一個核心問題。該問題的實質是在考慮沖擊的前提下沿指定路徑的時間最優(yōu)軌跡規(guī)劃問題，是機器人運動規(guī)劃領域的一個分支[1]。

經(jīng)過幾十年的研究，機器人沿指定路徑的時間最優(yōu)軌跡規(guī)劃問題已經(jīng)取得了大量研究成果[2]。Bobrow[3]提出了一種通過引入路徑坐標來建立參數(shù)空間的方法，從而把多維問題轉化成二維問題，大大降低了時間最優(yōu)軌跡規(guī)劃問題的復雜程度。Pfeiffer和Johanni[4]結合關節(jié)轉矩的限制條件和軌跡在參數(shù)空間的幾何性質，提出了一種通過求解轉換點尋找最優(yōu)軌跡的方法。Shiller[5]對軌跡中的奇異點進行針對性處理，提出了一種更加完善的算法。Verscheure等[6]提出一種直接轉化的方法，將尋找全局最優(yōu)軌跡的問題轉化為能夠直接使用成熟的求解器得到最優(yōu)解的二階錐規(guī)劃問題。Kong等[7]在此基礎上使用三次樣條曲線在參數(shù)空間中對最優(yōu)軌跡進行擬合，雖然在一定程度上增加了軌跡的執(zhí)行時間，但得到了更加平滑的軌跡。Reynoso等[8]使用凸松弛方法建立了考慮庫侖摩擦力和黏滯摩擦力的機器人完整動力學模型，在此基礎上進行軌跡規(guī)劃。

串聯(lián)機器人是機器人的一個重要分支，在汽車制造、電子電氣、 塑料加工、 機械加工、 化工等行業(yè)中有著非常廣泛的應用。串聯(lián)機器人的軌跡規(guī)劃是串聯(lián)機器人研究領域的一個重要研究方向，對于提高機器人的工作效率、保證工作環(huán)境的安全、提高產(chǎn)品質量以及降低機器人的能量損耗等目標的實現(xiàn)具有重要意義。有鑒于此，本文提出了一種機器人在笛卡爾空間中的無沖擊近似時間最優(yōu)軌跡規(guī)劃算法。

1 量的書寫規(guī)則

1.1 基于路徑坐標的參數(shù)空間建立

軌跡規(guī)劃時不考慮機器人末端執(zhí)行器的受力并忽略摩擦力的影響，機器人的動力學方程為：

(1)

運用文獻[3]的方法引入路徑坐標s，可建立q與s之間的函數(shù)關系如下：

(2)

(3)

其中：

m(s)=M(q(s))q′(s)

(4)

c(s)=M(q(s))q″(s)+C(q(s),q′(s))q′(s)

(5)

以機器人軌跡規(guī)劃執(zhí)行時間T作為目標函數(shù)，根據(jù)式(2)可得：

(6)

由此機器人時間最優(yōu)軌跡規(guī)劃問題可描述為:

(7)

1.2 軌跡規(guī)劃問題轉化為二階錐規(guī)劃問題

二階錐規(guī)劃屬于凸優(yōu)化的一個分支，現(xiàn)有算法和常規(guī)計算能力已足以可靠地求解大規(guī)模凸優(yōu)化問題。二階錐規(guī)劃的標準形式為：

(8)

將式(7)中的目標函數(shù)及約束條件轉化成二階錐規(guī)劃標準形式的過程包括以下幾個步驟。

(9)

從而可以將式(7)轉化為：

(10)

第二步：離散化。首先將路徑坐標離散成(K+1)個點：s0=0≤sk≤1=sK，k=0,…,K；然后在每個離散點取bk=b(sk)，其中：bk為b(s)在第k個離散點的函數(shù)值；sk為第k個離散點的橫坐標。

將曲線b(s)在區(qū)間[sk,sk+1]視為直線，則有：

(11)

從而可以將式(10)(目標函數(shù))的右半部分轉化為：

(12)

(13)

第三步：轉化成二階錐規(guī)劃問題。首先引入變量ck和dk，限制條件為：

(14)

(15)

從而有：

(16)

由式(14)和式(15)得：

(17)

(18)

綜上所述，最終得到的轉化成二階錐規(guī)劃形式的時間最優(yōu)軌跡規(guī)劃問題如下：

(19)

2 消除沖擊的軌跡規(guī)劃算法

求解式(19)中的二階錐規(guī)劃問題，可以得到滿足約束條件且執(zhí)行時間最短的軌跡。但由于時間最優(yōu)軌跡規(guī)劃問題中并未對轉矩隨時間的變化率進行約束，因此在某些點會存在轉矩變化率過大的問題，即機器人末端執(zhí)行時間最優(yōu)軌跡時存在沖擊。因此，本文在完成二階錐規(guī)劃求解出機器人末端執(zhí)行器時間最優(yōu)軌跡之后提出一種無沖擊近似時間最優(yōu)軌跡規(guī)劃算法，算法流程圖如圖1所示。

圖1 消除沖擊的算法流程圖

首先運用文獻[3]的方法建立參數(shù)空間，并將機器人關節(jié)輸出轉矩約束條件轉化為參數(shù)空間的約束條件；然后將軌跡規(guī)劃問題轉化成二階錐規(guī)劃的形式并使用求解器求出全局最優(yōu)解，此解為沒有考慮轉矩變化率的時間最優(yōu)軌跡，因此存在沖擊。為消除沖擊，首先計算出機器人各關節(jié)轉矩，建立描述沖擊情況的軌跡沖擊函數(shù)，并求出所有路徑點的沖擊函數(shù)值；然后設置沖擊點的篩選條件，從而得到原軌跡上沖擊點及其路徑坐標值。針對每個沖擊點，在參數(shù)空間中使用五次多項式對沖擊點所在優(yōu)化區(qū)間內的軌跡進行局部優(yōu)化處理，從而消除沖擊。處理完所有沖擊點后，考慮到使用五次多項式處理時對原軌跡的改動可能導致新的沖擊點的產(chǎn)生，因此重新計算軌跡沖擊函數(shù)值并判斷是否存在沖擊點；若依然存在沖擊點，則在確定新沖擊點路徑坐標之后重復之前對沖擊點進行局部處理的步驟，并一直循環(huán)，直到軌跡中不再存在沖擊點，最終得到無沖擊軌跡。

求解式(19)中的二階錐規(guī)劃問題后，根據(jù)其中τk的表達式可計算出時間最優(yōu)軌跡所有路徑點處各關節(jié)的轉矩值，本文將轉矩變化率的絕對值定義為沖擊，為描述沖擊變化情況，構造沖擊函數(shù)如下：

(20)

其中：

在得到軌跡上所有路徑點處的沖擊函數(shù)值之后，需要設置一定的條件來篩選出沖擊點。本文將篩選的閾值設置為最大沖擊函數(shù)值的1/M，將沖擊函數(shù)值大于此閾值的點作為沖擊點，M值可根據(jù)對軌跡的要求進行適當?shù)恼{整。通過篩選得到?jīng)_擊點后，根據(jù)其路徑坐標在b(s)曲線上找到對應點，并使用五次多項式對b(s)曲線上沖擊點所在的優(yōu)化區(qū)間進行局部處理。局部優(yōu)化處理的操作流程如圖2所示。

圖2 局部優(yōu)化處理流程圖

具體處理方法如下：

步驟1,確定優(yōu)化區(qū)間。所得沖擊點的路徑坐標是已知的，設kc為優(yōu)化半徑，即沖擊點左右兩邊路徑點的個數(shù)，以沖擊點為中心的(2kc+1)個離散點所在的區(qū)間即為優(yōu)化區(qū)間。

步驟2,判斷是否存在優(yōu)化區(qū)間重疊。判斷方法為對相鄰沖擊點的路徑坐標做差，若差值小于或等于2kc，則兩個沖擊點的優(yōu)化區(qū)間存在相互重疊，執(zhí)行步驟3；反之則不存在優(yōu)化區(qū)間重疊，執(zhí)行步驟4。

步驟3,重新定義優(yōu)化區(qū)間。若兩個相鄰沖擊點的優(yōu)化區(qū)間存在重疊，則將兩個優(yōu)化區(qū)間合并為一個，考慮到可能存在兩個以上沖擊點優(yōu)化區(qū)間重疊的情況，重復步驟2，判斷是否仍然存在優(yōu)化區(qū)間重疊的情況，若存在，則重復步驟3，直至循環(huán)條件不成立，執(zhí)行步驟4。

步驟4,構造五次多項式。構造五次多項式的邊界條件需要通過對原曲線上的離散點進行分析得到，由于對兩端點要求的邊界條件均為端點處的函數(shù)值、一階導數(shù)值及二階導數(shù)值，因此可分別求解。求解思路如下。

設五次多項式的表達式為：

b(s)=a0+a1s+a2s2+a3s3+a4s4+a5s5

(21)

對端點的約束條件為：

(22)

式中s0,sf分別為優(yōu)化區(qū)間起點和終點的路徑坐標，在每個沖擊點均為已知量，端點邊界條件已經(jīng)由之前離散點求出時，亦作為已知量，因此方程組未知量僅為6個自變量系數(shù)，此時具有6個未知數(shù)和6個方程的線性方程組可求得唯一解，從而得到用于此優(yōu)化區(qū)間的五次多項式。

步驟5,替換優(yōu)化區(qū)間內b(s)值。得到所求五次多項式后，將優(yōu)化區(qū)間內所有路徑點的路徑坐標代入多項式，從而得到優(yōu)化后的b(s)值，此處所得的b(s)值即可用于之后計算新軌跡在此區(qū)間內的轉矩值和軌跡執(zhí)行時間等參數(shù)。對偽加速度項a(s)求導可得：

(23)

步驟6,判斷循環(huán)結束條件。完成步驟5之后，對于一個沖擊點的所有處理操作已經(jīng)結束，所以接下來判斷所處理的沖擊點是否為所有沖擊點中的最后一個，若判斷結果為是，則針對沖擊點的優(yōu)化處理操作結束；若判斷結果為否，則讀取下一個沖擊點路徑坐標，并從步驟1開始重復循環(huán)，直至判斷結果為是。

至此，本文得到一條無沖擊的軌跡，機器人末端執(zhí)行此軌跡可保證運動平穩(wěn)無沖擊。

3 算例與結果分析

為驗證本文提出的軌跡規(guī)劃方法的有效性，本文以3R串聯(lián)機器人為研究對象，以機器人末端沿正弦曲線運動為例，在MATLAB中進行仿真計算。該機器人參數(shù)見表1。

表中m為連桿質量，r為連桿驅動關節(jié)與其質心之間的距離，JL和JT分別為各連桿相對于其質心在軸向和徑向的轉動慣量，l為關節(jié)之間的距離。連桿關節(jié)1,2,3的最大轉矩分別為140 N·m、140 N·m、50 N·m。

表1 3R機器人參數(shù)

機器人末端執(zhí)行器的路徑為三維空間中經(jīng)處理后的正弦曲線，如圖3所示，其參數(shù)方程為式(24),其中s′為自變量。

(24)

圖3 機器人末端路徑

首先以時間最優(yōu)軌跡規(guī)劃算法針對此路徑求解出機器人軌跡，然后根據(jù)式(19)中τk的表達式計算出機器人在所有路徑點處各關節(jié)轉矩，結果如圖4所示。由圖4可以看出，機器人執(zhí)行時間最優(yōu)軌跡時，各關節(jié)轉矩均存在突變，且關節(jié)2在大部分路徑點處輸出轉矩處于其極限值，且存在關節(jié)轉矩從最大值瞬間變化為最小值的情況，從而表明時間最優(yōu)軌跡存在沖擊。

圖4 時間最優(yōu)軌跡關節(jié)轉矩隨路徑坐標的變化情況

根據(jù)式(20)計算出此時間最優(yōu)軌跡所有路徑點的沖擊函數(shù)值，結果如圖5所示。由圖5可知，在大多數(shù)路徑點處，沖擊函數(shù)值較小，即關節(jié)轉矩變化率較小，不存在沖擊；在少數(shù)特殊路徑點處軌跡沖擊函數(shù)值突然增加，通過與圖4進行對比可知，此類路徑點處關節(jié)轉矩發(fā)生突變。

圖5 時間最優(yōu)軌跡沖擊函數(shù)值隨路徑坐標的變化情況

針對上述沖擊點，使用無沖擊近似時間最優(yōu)軌跡規(guī)劃算法進行局部優(yōu)化處理，得到無沖擊軌跡。根據(jù)式(20)計算無沖擊軌跡所有路徑點處的沖擊函數(shù)值，并與時間最優(yōu)軌跡的沖擊函數(shù)值進行對比，結果如圖6所示。

圖6 兩種軌跡沖擊函數(shù)值變化情況對比

由圖6可知，經(jīng)無沖擊近似時間最優(yōu)軌跡算法優(yōu)化處理后，無沖擊的路徑點處其沖擊函數(shù)值與時間最優(yōu)軌跡一致，即并未因優(yōu)化處理過程本身產(chǎn)生新的沖擊點；原沖擊點處的沖擊函數(shù)值明顯減小。

根據(jù)式(19)中τk的表達式可計算出無沖擊機器人在所有路徑點處各關節(jié)轉矩。無沖擊近似時間最優(yōu)軌跡規(guī)劃算法針對沖擊點進行局部優(yōu)化處理，對于無沖擊的路徑點，各關節(jié)轉矩與時間最優(yōu)軌跡一致，因此圖4與圖7差別不大。為直觀顯示機器人執(zhí)行兩條軌跡時關節(jié)轉矩的差別，選取3個典型的沖擊點，以其所在優(yōu)化區(qū)間內關節(jié)轉矩的差異實現(xiàn)兩條軌跡的對比。3個典型沖擊點依次為沖擊值最大的路徑點s1,沖擊值最小的路徑點s2以及沖擊函數(shù)值在所有沖擊點中位于中位數(shù)的路徑點s3。3個優(yōu)化區(qū)間δ1,δ2,δ3內的轉矩對比情況如圖7所示。

圖7 典型優(yōu)化區(qū)間內優(yōu)化前后關節(jié)轉矩對比

表2 3個典型優(yōu)化區(qū)間相關參數(shù)

對于式(24)所表示的路徑，使用MATLAB在相同的計算環(huán)境下(Windows 7操作系統(tǒng)，CPU 3.40 GHz)時間最優(yōu)軌跡的執(zhí)行時間為5.531 6 s，無沖擊軌跡的執(zhí)行時間為5.543 1 s，增加了0.21%(0.001 5 s)，因此無沖擊軌跡在消除了沖擊的情況下依然能夠保證近似最優(yōu)的執(zhí)行時間。

綜上所述，無沖擊近似時間最優(yōu)軌跡規(guī)劃算法能夠成功消除軌跡中的沖擊，并能夠保證軌跡時間的近似最優(yōu)，從而能夠實現(xiàn)機器人平穩(wěn)高效地運行。

4 結束語

本文以3R串聯(lián)型機器人為研究對象，對機器人時間最優(yōu)軌跡規(guī)劃問題進行了研究。首先通過建立參數(shù)空間把原本的多維優(yōu)化問題轉化到二維空間，然后將軌跡規(guī)劃問題轉化成二階錐規(guī)劃問題。在此基礎上提出了一種考慮沖擊的時間最優(yōu)軌跡規(guī)劃方法。該方法分3個步驟：第一步，建立沖擊函數(shù)并篩選出沖擊點；第二步，針對沖擊點使用三次樣條曲線在參數(shù)空間中對偽速度的平方項進行局部優(yōu)化；第三步，由優(yōu)化后的結果得到新的軌跡和各關節(jié)轉矩的變化情況。該方法最主要的特點是對沖擊點的篩選和針對性處理，從而使獲得的軌跡執(zhí)行時間短且運行平穩(wěn)。最后，在MATLAB環(huán)境下進行了仿真計算，設置機器人末端路徑為處理后的正弦函數(shù)曲線，通過對比優(yōu)化前后各關節(jié)轉矩的變化情況，驗證了此軌跡規(guī)劃方法的有效性。