郭 領(lǐng)

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

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

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

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

1 量的書(shū)寫(xiě)規(guī)則

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

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

(1)

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

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

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

(6)

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

(7)

1.2 軌跡規(guī)劃問(wèn)題轉(zhuǎn)化為二階錐規(guī)劃問(wèn)題

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

(8)

將式(7)中的目標(biāo)函數(shù)及約束條件轉(zhuǎn)化成二階錐規(guī)劃標(biāo)準(zhǔn)形式的過(guò)程包括以下幾個(gè)步驟。

(9)

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

(10)

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

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

(11)

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

(12)

(13)

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

(14)

(15)

從而有：

(16)

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

(17)

(18)

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

(19)

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

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

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

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

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

(20)

其中：

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

圖2 局部?jī)?yōu)化處理流程圖

具體處理方法如下：

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

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

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

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

設(shè)五次多項(xiàng)式的表達(dá)式為：

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

(21)

對(duì)端點(diǎn)的約束條件為：

(22)

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

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

(23)

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

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

3 算例與結(jié)果分析

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

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

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

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

(24)

圖3 機(jī)器人末端路徑

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

圖4 時(shí)間最優(yōu)軌跡關(guān)節(jié)轉(zhuǎn)矩隨路徑坐標(biāo)的變化情況

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

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

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

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

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

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

圖7 典型優(yōu)化區(qū)間內(nèi)優(yōu)化前后關(guān)節(jié)轉(zhuǎn)矩對(duì)比

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

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

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

4 結(jié)束語(yǔ)

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