蘇杰

（爾智自動(dòng)化科技（上海）有限公司，上海 201112）

協(xié)作機(jī)器人屬于典型的串聯(lián)機(jī)器人，其運(yùn)行過(guò)程中的振動(dòng)和機(jī)械沖擊對(duì)機(jī)器人精度和壽命有較大的影響。機(jī)器人軌跡規(guī)劃器的加速度不連續(xù)，會(huì)引起機(jī)器人運(yùn)行過(guò)程中的機(jī)械沖擊和末端抖動(dòng)。選擇雙S速度規(guī)劃器作為軌跡規(guī)劃器能夠避免出現(xiàn)機(jī)械沖擊，能夠提高機(jī)器人壽命、減少振動(dòng)、增加精度。

雙S速度規(guī)劃器是基于7段(典型值)加加速度(Jerk,j)為分段常量的曲線。由于引入了具有階躍函數(shù)特點(diǎn)的加加速度，使得加速度(Acceleration, a)線性連續(xù)變化，進(jìn)而控制速度(Velocity, v)平滑變化。因?yàn)榧铀俣冗B續(xù)，無(wú)加速度沖擊，關(guān)節(jié)執(zhí)行器在改變運(yùn)動(dòng)狀態(tài)過(guò)程中承受的沖擊力小，大幅度減小了機(jī)械系統(tǒng)的震動(dòng)，保證機(jī)器人運(yùn)動(dòng)速度高效的同時(shí)減小了對(duì)機(jī)械本體的沖擊。

由于系統(tǒng)的運(yùn)動(dòng)約束不同，雙S速度規(guī)劃器可能不完整包含典型的7段。根據(jù)系統(tǒng)給定的起點(diǎn)速度v0,終點(diǎn)速度v1,位移q，最大速度vmax，最大加速度amax,最大加加速度jmax,雙S速度規(guī)劃器可能不含勻速段、勻加速段和勻減速段，或者只有加速段或減速段之一的情況。因此在規(guī)劃判定后，雙S速度規(guī)劃器可能縮減至只有2段的情況。

用于機(jī)床數(shù)控系統(tǒng)的雙S速度規(guī)劃器基于系統(tǒng)配置和響應(yīng)的不同，有不少不同方向的研究，見(jiàn)文獻(xiàn)[6,7,8]。在機(jī)床數(shù)控領(lǐng)域，文獻(xiàn)[2]將不同約束和輸入?yún)?shù)產(chǎn)生的雙S速度規(guī)劃器分為8種情況，文獻(xiàn)[3]分為17種，已經(jīng)在分類計(jì)算上頗為詳細(xì)了。不過(guò)相比之下用于協(xié)作機(jī)器人的雙S速度規(guī)劃器研究較少，文獻(xiàn)[1,4]雖然提出了雙S速度規(guī)劃器在碼垛機(jī)器人和協(xié)作機(jī)器人上應(yīng)用，不過(guò)對(duì)于不同系統(tǒng)約束和輸入?yún)?shù)的情況沒(méi)有分類和判斷的研究，對(duì)于遇到極端值情況下的求解沒(méi)有研究。文獻(xiàn)[5]提出了三角函數(shù)速度曲線規(guī)劃，但是由于位置的三階導(dǎo)數(shù)是不連續(xù)的，很顯然也是達(dá)不到協(xié)作機(jī)器人軌跡規(guī)劃要求的。

由于起點(diǎn)速度v0，終點(diǎn)速度v1,位移q的任意性，可能存在給定的起止速度在最大加速度amax和最大加加速度jmax的約束條件下無(wú)法產(chǎn)生期望軌跡，或者說(shuō)為了滿足期望位移而無(wú)法達(dá)到期望速度，即存在最大速度不可達(dá)情況下軌跡不能求解并規(guī)劃的問(wèn)題。上述文獻(xiàn)對(duì)該問(wèn)題的解決沒(méi)有涉及，研究是不足的。

本文為了求解該問(wèn)題，對(duì)傳統(tǒng)雙S速度規(guī)劃器進(jìn)行改善。引入了加速度衰減迭代系數(shù)，提出了一種可以有效求解的算法。這種算法除了可以判斷加速度極值不可達(dá)的情況并可以對(duì)時(shí)間周期求解以外，還可以判斷加速度段和減速度段僅存其一的情況并可以對(duì)時(shí)間周期求解。因此全面覆蓋了各種情況下符合協(xié)作機(jī)器人要求的軌跡規(guī)劃曲線的求解，確保軌跡規(guī)劃的連續(xù)性和機(jī)器人運(yùn)行的平穩(wěn)性。之后在協(xié)作機(jī)器人進(jìn)行實(shí)施，驗(yàn)證改進(jìn)算法。

1 傳統(tǒng)雙S速度規(guī)劃器

圖1 傳統(tǒng)雙S速度規(guī)劃器軌跡圖

傳統(tǒng)雙S速度規(guī)劃器的軌跡規(guī)劃根據(jù)二次加速度變化分為典型的7段，根據(jù)加速度類型分為3個(gè)階段：加速段、勻速段和減速段。假定vlim=vmax，判斷勻加速段是否存在。首先求解加速度段時(shí)間Ta和位移qa，減速度段時(shí)間Td和位移qd，其次將加速段和減速段位移和期望位移比較，如果，則勻速段存在，反之勻速段不存在。在勻速段不存在的情況下，通過(guò)對(duì)vmax的解析和迭代，求解極限速度vlim，而在加速段和減速段不同時(shí)存在的情況下，再次迭代求解vlim，隨后反求加速段最大加速度alima、減速段最大加速度alimd和分段時(shí)間，最后由下述的軌跡規(guī)劃方程組(1)求解位移q(t)、速度v(t)、加速度a(t)：

在協(xié)作機(jī)器人軌跡規(guī)劃中，由于串聯(lián)式關(guān)節(jié)結(jié)構(gòu)，機(jī)器人有多個(gè)自由度，每個(gè)關(guān)節(jié)運(yùn)動(dòng)位移在不同的位姿下差別很大，因此在系統(tǒng)給定的插補(bǔ)位移很小而速度變化較大時(shí)，雙S速度規(guī)劃器是無(wú)效的。此時(shí)其計(jì)算方法不適合協(xié)作機(jī)器人軌跡規(guī)劃求解。因此在軌跡規(guī)劃中，首先要根據(jù)期望起點(diǎn)位置、期望終點(diǎn)位置、期望起點(diǎn)速度、期望終點(diǎn)速度，進(jìn)行如下判斷，以保證軌跡規(guī)劃有效后再進(jìn)行計(jì)算：

為了在協(xié)作機(jī)器人中使用雙S曲線軌跡規(guī)劃，本文首先解析最大速度vlim，進(jìn)而解析加速段最大加速度alima、減速段最大加速度alimd，解析過(guò)程中已經(jīng)求解了加加速段Tj1、加速段Ta、勻加速段Tj-2Tj1、勻速段Tv、減速段Td、加減速段Tj2、勻減速段Td-2Tj2的分段周期，解析完成后帶入方程組（1）求解。

2 改進(jìn)雙S速度規(guī)劃器

由于機(jī)器人關(guān)節(jié)執(zhí)行器在正反轉(zhuǎn)時(shí)的最大速度vmax和vmin、最大加速度amax和amin、最大二次加速度jmax和jmin是對(duì)稱的并且為常量，因此本文有如下約束：

2.1 存在勻速段的求解

2.1.1 最大加/減速度可達(dá)的判斷條件

2.1.2 求解 Ta和 Td

2.1.3 求解 Tv

如果Tv≥0，vmin可達(dá)，存在勻速段；反之a(chǎn)max不可達(dá)，不存在勻速段。

2.2 不存在勻速段的求解

假定加速度amax和減速度amin均可達(dá)：

如果Ta≥2Tj且Td≥2Tj，那么假定有效。如果Ta＜2Tj或Td＜2Tj，那么意味著amax不可達(dá)或amin不可達(dá)，此時(shí)引入衰減迭代系數(shù)。

2.3 加速度或減速度不可達(dá)

令amax=kamax，采用衰減迭代，根據(jù)(11)(12)(13)重新計(jì)算Ta和Td，直到Ta≥2Tj且Td≥2Tj后退出迭代，此時(shí)會(huì)得到軌跡規(guī)劃中Ta和Td的合適值。

在上述迭代算法計(jì)算過(guò)程中遇到Td或Td小于零的情況，如果Ta＜0，意味著沒(méi)有加速段，此時(shí)求解Td和Tj2，如果Td＜0，意味著沒(méi)有減速段，此時(shí)只需要計(jì)算Ta和Tj1：

2.4 vlim、alima、alimd 值的確定

vmax、amax、amin其一可達(dá)的情況，按下式確定極值：

vmax、amax、amin其一不可達(dá)的情況，按下式確定極值：

2.5 反向運(yùn)動(dòng)的軌跡規(guī)劃求解

2.6 程序流程圖（圖2）

圖2 程序流程圖

3 算法實(shí)驗(yàn)驗(yàn)證

3.1 驗(yàn)證設(shè)備

采用六自由度協(xié)作機(jī)器人對(duì)本文算法進(jìn)行應(yīng)用和驗(yàn)證。機(jī)器人控制系統(tǒng)分為中央控制層、通信層和驅(qū)控層。中央控制層主要進(jìn)行機(jī)械臂的正逆運(yùn)動(dòng)學(xué)計(jì)算、動(dòng)力學(xué)計(jì)算、坐標(biāo)轉(zhuǎn)換和軌跡規(guī)劃頂層控制。

中央控制系統(tǒng)硬件為研華MIO嵌入式單板工控機(jī)。關(guān)節(jié)執(zhí)行器采用高度集成化設(shè)計(jì)并集減速器、電機(jī)、驅(qū)動(dòng)器、帶散熱功能的制動(dòng)器、編碼器等組件于一體的驅(qū)控一體化關(guān)節(jié)模組。本文算法在中央控制層進(jìn)行計(jì)算，輸出的軌跡規(guī)劃參數(shù)求解后下發(fā)給各關(guān)節(jié)控制器。

3.2 算法驗(yàn)證

以觀測(cè)效果比較明顯的第二關(guān)節(jié)進(jìn)行驗(yàn)證。關(guān)節(jié)最大速度vmax=180°/s，最大加/減速度amax=360°/s,最大加加速度 jmax=2160°/s。

3.2.1 速度不可達(dá)，加速度均可達(dá)

圖3 速度不可達(dá)，加速度均可達(dá)的軌跡規(guī)劃曲線

驗(yàn)證機(jī)器人運(yùn)動(dòng)軌跡結(jié)果如圖4。

圖4 速度不可達(dá)，加速度均可達(dá)的真機(jī)驗(yàn)證

3.2.2 速度和加速度均不可達(dá)

圖5 速度和加速度均不可達(dá)的軌跡規(guī)劃曲線

圖6 速度和加速度均不可達(dá)的真機(jī)驗(yàn)證

4 結(jié)語(yǔ)

本文研究了適合協(xié)作機(jī)器人雙S速度規(guī)劃器的算法應(yīng)用，詳細(xì)討論了不同情況下規(guī)劃器的參數(shù)求解問(wèn)題并給出了解析方式和計(jì)算方法。通過(guò)引入衰減迭代系數(shù)，進(jìn)行一次性局部衰減迭代計(jì)算，解決了速度規(guī)劃器在最大速度不可達(dá)情況下，如何高效精確地求解加/減速度段時(shí)間的問(wèn)題。

通過(guò)對(duì)速度不可達(dá)而加速度均可達(dá)情況下的算法仿真驗(yàn)證了本文算法的可靠性和可行性。進(jìn)一步在速度和加速度均不可達(dá)情況下通過(guò)引入衰減迭代系數(shù)k，驗(yàn)證了求解加/減速度段時(shí)間的高效、精確和可行性。

最后將本文算法在協(xié)作機(jī)器人系統(tǒng)中進(jìn)行真機(jī)驗(yàn)證，機(jī)器人啟停、加減速運(yùn)動(dòng)及軌跡算法切換均無(wú)抖動(dòng)，機(jī)器人運(yùn)行平穩(wěn)。表明了本文算法在協(xié)作機(jī)器人軌跡規(guī)劃中的有效和可靠性。