一種串聯機器人笛卡爾空間軌跡規劃方法

郭 領

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

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

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

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

1 量的書寫規則

1.1 基于路徑坐標的參數空間建立

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

(1)

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

(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)

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

(6)

由此機器人時間最優軌跡規劃問題可描述為:

(7)

1.2 軌跡規劃問題轉化為二階錐規劃問題

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

(8)

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

(9)

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

(10)

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

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

(11)

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

(12)

(13)

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

(14)

(15)

從而有：

(16)

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

(17)

(18)

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

(19)

2 消除沖擊的軌跡規劃算法

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

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

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

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

(20)

其中：

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

圖2 局部優化處理流程圖

具體處理方法如下：

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

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

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

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

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

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

(21)

對端點的約束條件為：

(22)

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

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

(23)

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

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

3 算例與結果分析

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

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

表1 3R機器人參數

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

(24)

圖3 機器人末端路徑

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

圖4 時間最優軌跡關節轉矩隨路徑坐標的變化情況

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

圖5 時間最優軌跡沖擊函數值隨路徑坐標的變化情況

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

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

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

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

圖7 典型優化區間內優化前后關節轉矩對比

表2 3個典型優化區間相關參數

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

綜上所述，無沖擊近似時間最優軌跡規劃算法能夠成功消除軌跡中的沖擊，并能夠保證軌跡時間的近似最優，從而能夠實現機器人平穩高效地運行。

4 結束語

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