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

寧學濤，黃 偉，鄭天江，楊亞威

（1.中國科學院寧波材料技術與工程研究所，寧波 315201；2.中北大學，太原 030000）

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

寧學濤1,2，黃 偉1,2，鄭天江1，楊亞威1

（1.中國科學院寧波材料技術與工程研究所，寧波 315201；2.中北大學，太原 030000）

0 引言

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

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

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

1 工作空間關鍵點的選取

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

圖1 工作空間關鍵點的選取

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

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

圖2 關鍵點處速度較小

圖3 關鍵點處速度較大

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

圖4 關鍵點處加速度較小

圖5 關鍵點處加速度較大

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

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

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

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

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

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

擬合曲線1獨有方程式：

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

擬合曲線2獨有方程式：

擬合曲線n獨有方程式：

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

其中：

是36×36的時間矩陣；

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

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

3 動力學軌跡規(guī)劃優(yōu)化模型

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

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

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

4 擬合曲線分析

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

圖7 左右驅(qū)動關節(jié)運動學擬合曲線

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

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

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

圖9 左右驅(qū)動關節(jié)動力學擬合曲線

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

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

5 實驗結(jié)果分析

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

圖10 左邊驅(qū)動電機位移圖

圖11 右邊驅(qū)動電機位移圖

圖12 左邊驅(qū)動電機速度圖

圖13 右邊驅(qū)動電機速度圖

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

6 結(jié)束語

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

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

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

[3]凌家良,施榮華,王國才.工業(yè)機器人關節(jié)空間的差值軌跡規(guī)劃[J].惠州學院學報,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

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

寧學濤（1989 -），男，河北唐山人，碩士研究生，研究方向為高速并聯(lián)工業(yè)機器人。

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

機器人；運動學；動力學；混合空間；軌跡規(guī)劃

TP242

A

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

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

2015-04-05

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