寧學(xué)濤，黃 偉，鄭天江，楊亞威

（1.中國(guó)科學(xué)院寧波材料技術(shù)與工程研究所，寧波 315201；2.中北大學(xué)，太原 030000）

關(guān)節(jié)空間和工作空間的混合軌跡規(guī)劃算法研究

寧學(xué)濤1,2，黃 偉1,2，鄭天江1，楊亞威1

（1.中國(guó)科學(xué)院寧波材料技術(shù)與工程研究所，寧波 315201；2.中北大學(xué)，太原 030000）

0 引言

軌跡規(guī)劃對(duì)于提高機(jī)器人的使用效率，減小工作過程中的振動(dòng)具有重要意義。合理的軌跡規(guī)劃能夠使驅(qū)動(dòng)電機(jī)力矩和功率得到的充分的利用，并從軟件控制上提高機(jī)器人的整體性能。

華南理工大學(xué)的陳偉華等人[1]，對(duì)機(jī)器人的連續(xù)路徑進(jìn)行了工作空間內(nèi)的軌跡規(guī)劃，在路徑的拐角處采用關(guān)節(jié)空間軌跡規(guī)劃進(jìn)行過渡，但是，并沒有進(jìn)行相應(yīng)的動(dòng)力學(xué)優(yōu)化。同濟(jì)大學(xué)的李萬莉等人[2]，江南大學(xué)的凌家良等人[3]，進(jìn)行了純關(guān)節(jié)空間內(nèi)的軌跡規(guī)劃，也沒有進(jìn)行相應(yīng)的動(dòng)力學(xué)優(yōu)化。

對(duì)于每個(gè)運(yùn)行階段，機(jī)器人都有不同的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)要求，例如，對(duì)于本文中所述的Delta兩自由度高速并聯(lián)工業(yè)機(jī)器人，在豎直方向物體抓取和釋放階段，要求機(jī)器人末端執(zhí)行器應(yīng)盡量減小水平方向的抖動(dòng)，同時(shí)應(yīng)盡量減小關(guān)節(jié)空間所需驅(qū)動(dòng)力矩和功率，并且減小水平方向的抖動(dòng)應(yīng)作為這一階段的主要矛盾加以解決，即擁有良好的工作空間性能；在水平轉(zhuǎn)運(yùn)階段和中間過渡階段，要求減小關(guān)節(jié)空間驅(qū)動(dòng)力矩和功率，同時(shí)應(yīng)盡量減小關(guān)節(jié)空間的輸入角速度，即擁有良好的關(guān)節(jié)空間性能。由于純工作空間和純關(guān)節(jié)空間軌跡規(guī)劃得到的擬合曲線一般具有良好的工作空間或關(guān)節(jié)空間性能，為了從理論上提高機(jī)器人的性能，本文將對(duì)機(jī)器人的豎直物體抓取和釋放階段采用工作空間五次樣條函數(shù)軌跡規(guī)劃方法，對(duì)水平轉(zhuǎn)運(yùn)階段和中間過渡階段采用關(guān)節(jié)空間五次樣條函數(shù)軌跡規(guī)劃方法，最后通過實(shí)驗(yàn)驗(yàn)證方法的合理性。

1 工作空間關(guān)鍵點(diǎn)的選取

綜合，工作空間和關(guān)節(jié)空間軌跡規(guī)劃中工作空間關(guān)鍵點(diǎn)的需求，在工作空間選取了9個(gè)工作空間關(guān)鍵點(diǎn)，如圖1所示，其中曲線01、78段為工作空間軌跡規(guī)劃段，該階段是前面所述的物體抓取和釋放曲線段，應(yīng)具有良好的工作空間性能，即應(yīng)盡量減小水平方向的抖動(dòng)。其余曲線段為關(guān)節(jié)空間軌跡規(guī)劃階段，該階段要求機(jī)器人具有良好的關(guān)節(jié)空間驅(qū)動(dòng)力矩和功率，由于該階段的關(guān)鍵點(diǎn)對(duì)稱布置，得到的工作空間擬合曲線沒有較大的抖動(dòng)。

圖1 工作空間關(guān)鍵點(diǎn)的選取

關(guān)鍵點(diǎn)2、6為工作空間拐彎半徑控制點(diǎn)，對(duì)該兩點(diǎn)的位置進(jìn)行調(diào)節(jié)不僅可以改變拐彎半徑，還可以調(diào)節(jié)23、34、45、56曲線段豎直方向的抖動(dòng)；關(guān)鍵點(diǎn)1、7為工作空間軌跡規(guī)劃和關(guān)節(jié)空間軌跡規(guī)劃的銜接點(diǎn)，在該兩點(diǎn)處應(yīng)該選擇合理的工作空間速度和加速度數(shù)值，以求得到較好的工作空間和關(guān)節(jié)空間擬合曲線。

現(xiàn)只對(duì)其中一個(gè)關(guān)鍵點(diǎn)數(shù)值選取進(jìn)行陳述，另一關(guān)鍵點(diǎn)數(shù)值選取與該關(guān)鍵點(diǎn)類似。當(dāng)速度選擇不合理時(shí)，該關(guān)鍵點(diǎn)（時(shí)間為0.2s，藍(lán)色曲線和紅色曲線銜接處）成為速度擬合曲線的尖點(diǎn)（即從曲線上明顯突出的點(diǎn)），并且該關(guān)鍵點(diǎn)附近的加速度急劇變化、數(shù)值較大，如圖2、圖3所示，分別為關(guān)鍵點(diǎn)處速度選擇較小、較大時(shí)的關(guān)節(jié)空間左驅(qū)動(dòng)電機(jī)擬合曲線。

圖2 關(guān)鍵點(diǎn)處速度較小

圖3 關(guān)鍵點(diǎn)處速度較大

當(dāng)加速度選擇不合理時(shí)，該關(guān)鍵點(diǎn)成為速度曲線的曲率變化關(guān)鍵點(diǎn)，并且該關(guān)鍵點(diǎn)的加速度出現(xiàn)尖點(diǎn)，如圖4、圖5所示，分別為關(guān)鍵點(diǎn)處加速度選擇較小、較大時(shí)的關(guān)節(jié)空間左驅(qū)動(dòng)電機(jī)擬合曲線。

圖4 關(guān)鍵點(diǎn)處加速度較小

圖5 關(guān)鍵點(diǎn)處加速度較大

當(dāng)關(guān)鍵點(diǎn)處的速度、加速度選擇不合理時(shí)，還會(huì)出現(xiàn)工作空間位移擬合曲線過沖，速度擬合曲線抖動(dòng)較大，加速度擬合曲線峰值較大，所需驅(qū)動(dòng)電機(jī)力矩峰值和功率峰值急劇增加的現(xiàn)象，這里不再贅述。

2 混合軌跡規(guī)劃五次樣條函數(shù)模型

混合軌跡規(guī)劃五次樣條函數(shù)模型是工作空間軌跡規(guī)劃和關(guān)節(jié)空間軌跡規(guī)劃相結(jié)合的數(shù)學(xué)模型，工作空間軌跡規(guī)劃曲線段01、78段使用工作空間軌跡規(guī)劃數(shù)學(xué)模型，工作空間的五次樣條函數(shù)軌跡規(guī)劃數(shù)學(xué)模型如式(1)所示。

通過計(jì)算得到五次樣條函數(shù)系數(shù)如式(3)所示：

為了使每段關(guān)節(jié)空間內(nèi)關(guān)節(jié)角位移、速度、加速度、加加速度擬合曲線連接處連續(xù)可導(dǎo)，需要建立合理的邊界條件。Delta兩自由度高速并聯(lián)工業(yè)機(jī)器人有兩個(gè)關(guān)節(jié)輸入量，現(xiàn)只對(duì)關(guān)節(jié)角1的邊界條件進(jìn)行闡述，關(guān)節(jié)角2的邊界條件與關(guān)節(jié)角1相同，不同之處在于工作空間內(nèi)7個(gè)關(guān)鍵點(diǎn)經(jīng)過運(yùn)動(dòng)學(xué)逆解得到的關(guān)節(jié)空間內(nèi)的7關(guān)鍵點(diǎn)的數(shù)值不同。根據(jù)已經(jīng)給定的關(guān)節(jié)角1的已知量和每段擬合曲線的邊界條件建立如下方程式。

過境貨物分為直接過境和間接過境，而其中間接過境的貨物和準(zhǔn)備在國(guó)內(nèi)銷售的貨物不一樣，屬于尚未進(jìn)口的貨物，根據(jù)我國(guó)《海關(guān)知識(shí)產(chǎn)權(quán)保護(hù)條例》的規(guī)定，我們只對(duì)進(jìn)口貨物有執(zhí)法權(quán)，但在自貿(mào)區(qū)內(nèi)，國(guó)內(nèi)海關(guān)對(duì)于進(jìn)口的貨物卻無執(zhí)法權(quán)，因?yàn)檎咭?guī)定進(jìn)入海關(guān)的貨物免于辦理申報(bào)手續(xù)，國(guó)內(nèi)海關(guān)不可以采取知識(shí)產(chǎn)權(quán)保護(hù)措施的問題便相繼出現(xiàn)。鑒于此，重慶自貿(mào)區(qū)應(yīng)該在其以全方位開放為主、貨物和監(jiān)管更加便利等特殊性的條件下，建立一個(gè)區(qū)別于傳統(tǒng)自貿(mào)區(qū)的獨(dú)特模式。因此自貿(mào)區(qū)內(nèi)的過境貨物應(yīng)視為進(jìn)口貨物，統(tǒng)一過境貨物執(zhí)法措施。

擬合曲線1獨(dú)有方程式：

擬合曲線1、2共有方程式：

擬合曲線2獨(dú)有方程式：

擬合曲線n獨(dú)有方程式：

將得到關(guān)節(jié)空間內(nèi)從關(guān)鍵點(diǎn)1到關(guān)鍵點(diǎn)7的7個(gè)點(diǎn)的含有36個(gè)未知量的36個(gè)關(guān)于時(shí)間的線性方程，對(duì)其進(jìn)行整理得到矩陣：B=KA。

其中：

是36×36的時(shí)間矩陣；

A為待求的方程組的系數(shù)；

系數(shù)矩陣A=K-1B，通過計(jì)算得到了關(guān)節(jié)角1的擬合曲線的系數(shù)矩陣A，即得到了關(guān)節(jié)角1從關(guān)鍵點(diǎn)1到關(guān)鍵點(diǎn)7的擬合曲線，同理可得到關(guān)節(jié)角1從關(guān)鍵點(diǎn)7到關(guān)鍵點(diǎn)1的擬合曲線，關(guān)節(jié)角2的擬合方法與關(guān)節(jié)角1類似，不再贅述。

3 動(dòng)力學(xué)軌跡規(guī)劃優(yōu)化模型

混合軌跡規(guī)劃的動(dòng)力學(xué)優(yōu)化模型，結(jié)合了工作空間軌跡規(guī)劃的動(dòng)力學(xué)優(yōu)化模型和關(guān)節(jié)空間軌跡規(guī)劃的動(dòng)力學(xué)優(yōu)化模型，在工作空間軌跡規(guī)劃01、78曲線段使用工作空間動(dòng)力學(xué)軌跡優(yōu)化數(shù)學(xué)模型進(jìn)行動(dòng)力學(xué)優(yōu)化；其余關(guān)節(jié)空間軌跡規(guī)劃曲線段使用關(guān)節(jié)空間的動(dòng)力學(xué)軌跡優(yōu)化數(shù)學(xué)模型進(jìn)行動(dòng)力學(xué)優(yōu)化。

圖6 混合軌跡規(guī)劃流程圖

Delta機(jī)器人混合空間軌跡規(guī)劃流程如圖6所示，其中判斷1為選取的工作空間關(guān)鍵點(diǎn)處速度、加速度是否合理，即關(guān)節(jié)空間中擬合曲線，速度是否為尖點(diǎn)，并且加速度是否急劇變化、數(shù)值較大；工作空間中擬合曲線，位移是否過沖，速度擬合曲線抖動(dòng)是否較大，加速度擬合曲線峰值是否較大，以及所需驅(qū)動(dòng)電機(jī)力矩峰值和功率峰值是否急劇增加、數(shù)值較大。判斷2為得到的工作空間內(nèi)末端執(zhí)行器的速度是否小于等于速度所要求的最大峰值，為了增加機(jī)器人的末端執(zhí)行器的運(yùn)行速度，應(yīng)盡量使末端執(zhí)行器的速度維持在峰值；判斷3為得到的工作空間內(nèi)的末端執(zhí)行器的加速度是否小于等于加速度所要求的最大峰值。判斷4為得到的關(guān)節(jié)空間內(nèi)的擬合曲線的關(guān)節(jié)角位移、速度是否沒有過沖，速度、加速度、加加速度擬合曲線峰值是否相差較小；判斷5為得到的關(guān)節(jié)驅(qū)動(dòng)力矩峰值和功率峰值是否相差較小，為了充分利用驅(qū)動(dòng)電機(jī)的性能，應(yīng)盡量使每條擬合曲線段內(nèi)的關(guān)節(jié)驅(qū)動(dòng)力矩或驅(qū)動(dòng)功率維持在峰值附近。以上判別量都是時(shí)間的函數(shù)，修改每段擬合曲線的時(shí)間即可對(duì)運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的擬合曲線進(jìn)行整體優(yōu)化。

4 擬合曲線分析

根據(jù)Delta機(jī)器人工作空間和關(guān)節(jié)空間的混合軌跡規(guī)劃五次樣條函數(shù)模型及其動(dòng)力學(xué)優(yōu)化模型，編寫機(jī)器人的Python語言混合空間軌跡規(guī)劃程序，得到的擬合曲線如圖7～圖9所示，其中，藍(lán)色、黑色曲線為工作空間軌跡規(guī)劃得到的擬合曲線，紅色、綠色曲線為關(guān)節(jié)空間軌跡規(guī)劃得到的擬合曲線。

圖7 左右驅(qū)動(dòng)關(guān)節(jié)運(yùn)動(dòng)學(xué)擬合曲線

圖7為混和軌跡規(guī)劃法得到的關(guān)節(jié)空間內(nèi)左右驅(qū)動(dòng)關(guān)節(jié)運(yùn)動(dòng)學(xué)擬合曲線，由上至下分別表示驅(qū)動(dòng)關(guān)節(jié)角位移、速度、加速度。由圖可以看出，關(guān)節(jié)空間內(nèi)的位移、速度擬合曲線均連續(xù)可導(dǎo)，加速度擬合曲線連續(xù)但不可導(dǎo)。左右驅(qū)動(dòng)關(guān)節(jié)的速度大小均小于8rad/s，左驅(qū)動(dòng)關(guān)節(jié)加速度峰值小于200rad/s2，右驅(qū)動(dòng)關(guān)節(jié)加速度峰值小于150rad/s2。

圖8 末端執(zhí)行器擬合曲線

圖8為混合軌跡規(guī)劃法得到的工作空間內(nèi)末端執(zhí)行器x軸方向和y軸方向擬合曲線，由上至下分別表示末端執(zhí)行器的位移、速度、加速度擬合曲線。由圖可以看出，利用以上混合軌跡規(guī)劃五次樣條函數(shù)模型及其動(dòng)力學(xué)優(yōu)化模型，得到的工作空間內(nèi)末端執(zhí)行器的位移、速度擬合曲線均連續(xù)可導(dǎo)，加速度擬合曲線連續(xù)但不可導(dǎo)，擬合曲線x軸方向和y軸方向速度峰值大小約為3m/s，加速度峰值大小為60m/s2，得到的工作空間內(nèi)的x、y軸方向速度、加速度擬合曲線的峰值相差較小。

圖9 左右驅(qū)動(dòng)關(guān)節(jié)動(dòng)力學(xué)擬合曲線

圖9為混合軌跡規(guī)劃法動(dòng)力學(xué)優(yōu)化后得到的關(guān)節(jié)空間內(nèi)驅(qū)動(dòng)電機(jī)力矩和功率擬合曲線。由圖可知，左右驅(qū)動(dòng)關(guān)節(jié)力矩?cái)M合曲線和功率擬合曲線均連續(xù)但不可導(dǎo)，左驅(qū)動(dòng)關(guān)節(jié)力矩?cái)M合曲線峰值大小小于80N.m，右驅(qū)動(dòng)關(guān)節(jié)力矩?cái)M合曲線峰值大小小于等于90N.m，左右關(guān)節(jié)驅(qū)動(dòng)力矩峰值大小相差較小；左右驅(qū)動(dòng)電機(jī)的功率擬合曲線為取絕對(duì)值后的擬合曲線，左驅(qū)動(dòng)關(guān)節(jié)功率擬合曲線峰值大小小于400w，右驅(qū)動(dòng)關(guān)節(jié)功率擬合曲線峰值大小小于550w，左右關(guān)節(jié)驅(qū)動(dòng)功率峰值大小相差較大。

由Delta機(jī)器人的混合軌跡規(guī)劃五次樣條函數(shù)模型及其動(dòng)力學(xué)優(yōu)化模型得到的擬合曲線分析可知，在末端執(zhí)行器豎直抓取和釋放物體01、78曲線段，將末端執(zhí)行器水平x方向是否抖動(dòng)作為主要衡量指標(biāo)，將末端執(zhí)行器y方向速度極值大小，關(guān)節(jié)空間速度、加速度大小，驅(qū)動(dòng)電機(jī)力矩極值大小和功率極值大小等作為次要衡量指標(biāo)，可知混合軌跡規(guī)劃法得到的工作空間內(nèi)末端執(zhí)行器豎直運(yùn)行階段擬合曲線明顯好于關(guān)節(jié)空間軌跡規(guī)劃法得到的擬合曲線，這將十分有利于提高抓取和釋放物體時(shí)末端執(zhí)行器的穩(wěn)定性；在末端執(zhí)行器的水平轉(zhuǎn)運(yùn)和中間過渡階段，將末端執(zhí)行器x方向速度極值大小，關(guān)節(jié)空間速度、加速度大小，驅(qū)動(dòng)電機(jī)力矩極值大小和功率極值大小等作為主要衡量指標(biāo)，將末端執(zhí)行器y方向是否抖動(dòng)作為次要衡量指標(biāo)，可知混合軌跡規(guī)劃法得到的工作空間內(nèi)末端執(zhí)行器水平轉(zhuǎn)運(yùn)和中間過渡階段擬合曲線明顯好于工作空間軌跡規(guī)劃法得到的擬合曲線，這將十分有利于提高Delta機(jī)器人的實(shí)際控制性能，同時(shí)降低了所需驅(qū)動(dòng)電機(jī)的力矩和功率，具有很強(qiáng)的實(shí)用價(jià)值。

5 實(shí)驗(yàn)結(jié)果分析

為了對(duì)混合軌跡規(guī)劃程序結(jié)果的實(shí)際應(yīng)用價(jià)值進(jìn)行驗(yàn)證，從Copley交流伺服電機(jī)驅(qū)動(dòng)器中讀取了驅(qū)動(dòng)電機(jī)的實(shí)際運(yùn)行參數(shù)，圖10、圖11中紫紅色曲線分別為左右驅(qū)動(dòng)電機(jī)的位移曲線。從圖中可以看出，利用混合空間軌跡規(guī)劃法得到的驅(qū)動(dòng)電機(jī)的實(shí)際位移曲線均連續(xù)可導(dǎo)，與理論規(guī)劃結(jié)果圖7對(duì)比可知，實(shí)際的位移曲線對(duì)時(shí)間軸做了翻轉(zhuǎn)，但兩曲線的趨勢(shì)相同。

圖10 左邊驅(qū)動(dòng)電機(jī)位移圖

圖11 右邊驅(qū)動(dòng)電機(jī)位移圖

圖12 左邊驅(qū)動(dòng)電機(jī)速度圖

圖13 右邊驅(qū)動(dòng)電機(jī)速度圖

圖12、圖13為左右驅(qū)動(dòng)電機(jī)速度圖，綠色曲線為軌跡規(guī)劃指令曲線，紫紅色曲線為驅(qū)動(dòng)電機(jī)的實(shí)際速度曲線，與理論規(guī)劃結(jié)果圖7對(duì)比可知，測(cè)得的速度曲線對(duì)時(shí)間軸做了翻轉(zhuǎn)，實(shí)際速度曲線稍有抖動(dòng)，相對(duì)于純工作空間或純關(guān)節(jié)空間軌跡規(guī)劃得到的擬合曲線，工作空間和關(guān)節(jié)空間軌跡規(guī)劃得到的擬合曲線更有利于提高機(jī)器人末端執(zhí)行器和機(jī)器人的實(shí)際控制性能。

6 結(jié)束語

本文以提高工作空間末端執(zhí)行器的物體抓取和釋放曲線段工作空間擬合曲線與工作空間水平轉(zhuǎn)運(yùn)和過渡階段的關(guān)節(jié)空間擬合曲線性能為目標(biāo)，旨在于利用工作空間和關(guān)節(jié)空間軌跡規(guī)劃的優(yōu)點(diǎn)進(jìn)行混合空間的軌跡規(guī)劃，以此提高機(jī)器人的性能，并由得到的擬合曲線和實(shí)驗(yàn)證明了該方法的合理性。

[1]陳偉華,張鐵,崔敏其.基于五次多項(xiàng)式過渡的機(jī)器人軌跡規(guī)劃的研究[J].煤礦機(jī)械,2011,32(12):49-50.

[2]李萬莉,陳熙巍,茹蘭.基于SimMechanics的4自由度機(jī)器人的軌跡規(guī)劃和仿真系統(tǒng)設(shè)計(jì)[J].中國(guó)工程機(jī)械學(xué)報(bào),2008,6(2):144-148.

[3]凌家良,施榮華,王國(guó)才.工業(yè)機(jī)器人關(guān)節(jié)空間的差值軌跡規(guī)劃[J].惠州學(xué)院學(xué)報(bào),2009,29(3):52-57.

[4]Liu, I. H., Tsai,M. C.,Cheng, M. Y.,& Su, K. H. Planning and implementation of motion trajectory based on C 2 PH spline[A].IEEE Industrial Electronics Society[C].2011:246-251.

[5]Gasparetto A, Zanotto V. Optimal trajectory planning for industrial robots[J].Advances in Engineering Software, 2010,41(4):548-556.

[6]G.B. Zhou, B.R. Song, J.L. Xie. Numerical calculation[M].Beijing. Higher Education Press,2008.

Mixed joint space and work space trajectory planning algorithm

NING Xue-tao1,2, HUANG Wei1,2, ZHENG Tian-jiang1, YANG Ya-wei1

為了實(shí)現(xiàn)機(jī)器人的高速、高精度控制，必須對(duì)機(jī)器人進(jìn)行合理的軌跡規(guī)劃，同時(shí)，軌跡規(guī)劃也

寧學(xué)濤（1989 -），男，河北唐山人，碩士研究生，研究方向?yàn)楦咚俨⒙?lián)工業(yè)機(jī)器人。

是實(shí)現(xiàn)驅(qū)動(dòng)電機(jī)力矩和功率得到充分利用的核心內(nèi)容。將編寫Python語言的軌跡規(guī)劃程序，并使用關(guān)節(jié)空間和工作空間的五次樣條函數(shù)混合軌跡規(guī)劃法對(duì)Delta兩自由度機(jī)器人進(jìn)行軌跡規(guī)劃，再利用動(dòng)力學(xué)對(duì)軌跡規(guī)劃結(jié)果進(jìn)行相應(yīng)的優(yōu)化。生成的擬合曲線表明，機(jī)器人的混合軌跡規(guī)劃法充分利用了關(guān)節(jié)空間和工作空間軌跡規(guī)劃法的優(yōu)點(diǎn)，并且動(dòng)力學(xué)優(yōu)化后的結(jié)果，對(duì)提高驅(qū)動(dòng)電機(jī)的使用效率具有很好的指導(dǎo)性意義，最后通過實(shí)驗(yàn)驗(yàn)證了結(jié)果的正確性。

機(jī)器人；運(yùn)動(dòng)學(xué)；動(dòng)力學(xué)；混合空間；軌跡規(guī)劃

TP242

A

1009-0134(2015)07(下)-0020-05

10.3969/j.issn.1009-0134.2015.07(下).06

2015-04-05

寧波市國(guó)際合作項(xiàng)目（2014D10008）；數(shù)控一代機(jī)械產(chǎn)品創(chuàng)新應(yīng)用示范工程（2014BAZ04784）；精密驅(qū)動(dòng)控制技術(shù)創(chuàng)新團(tuán)隊(duì)（2012B82005）；寧波市自然科學(xué)基金（2014A610084）