關節空間和工作空間的混合軌跡規劃算法研究

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

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

關節空間和工作空間的混合軌跡規劃算法研究

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

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

0 引言

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

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

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

1 工作空間關鍵點的選取

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

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

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

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

圖2 關鍵點處速度較小

圖3 關鍵點處速度較大

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

圖4 關鍵點處加速度較小

圖5 關鍵點處加速度較大

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

2 混合軌跡規劃五次樣條函數模型

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

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

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

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

擬合曲線1獨有方程式：

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

擬合曲線2獨有方程式：

擬合曲線n獨有方程式：

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

其中：

是36×36的時間矩陣；

A為待求的方程組的系數；

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

3 動力學軌跡規劃優化模型

混合軌跡規劃的動力學優化模型，結合了工作空間軌跡規劃的動力學優化模型和關節空間軌跡規劃的動力學優化模型，在工作空間軌跡規劃01、78曲線段使用工作空間動力學軌跡優化數學模型進行動力學優化；其余關節空間軌跡規劃曲線段使用關節空間的動力學軌跡優化數學模型進行動力學優化。

圖6 混合軌跡規劃流程圖

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

4 擬合曲線分析

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

圖7 左右驅動關節運動學擬合曲線

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

圖8 末端執行器擬合曲線

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

圖9 左右驅動關節動力學擬合曲線

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

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

5 實驗結果分析

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

圖10 左邊驅動電機位移圖

圖11 右邊驅動電機位移圖

圖12 左邊驅動電機速度圖

圖13 右邊驅動電機速度圖

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

6 結束語

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

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

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

[3]凌家良,施榮華,王國才.工業機器人關節空間的差值軌跡規劃[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

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

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

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

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

TP242

A

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

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

2015-04-05

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