于常娟，張明路，朱敦雨，張世紅

(1. 廊 坊師范學(xué)院數(shù)信學(xué)院，河北廊坊065000;2. 河北工業(yè)大學(xué)機(jī)械工程學(xué)院，天津300130)

0 前言

軌跡規(guī)劃可以在操作空間中進(jìn)行，也可以在關(guān)節(jié)空間中進(jìn)行。第一種方法足端經(jīng)過(guò)的路徑和姿態(tài)直觀，當(dāng)運(yùn)動(dòng)空間中有障礙物存在時(shí)較容易判斷是否可行，但是它沒(méi)考慮運(yùn)動(dòng)的奇異性問(wèn)題，而且由于需要頻繁計(jì)算逆解，所以要求運(yùn)動(dòng)學(xué)逆解簡(jiǎn)單。第二種方法通過(guò)運(yùn)動(dòng)學(xué)逆解方程，求得與笛卡兒空間中較少的位置時(shí)間序列對(duì)應(yīng)的關(guān)節(jié)空間中的位置時(shí)間序列，然后進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃，這樣既避免了運(yùn)動(dòng)奇異性問(wèn)題，又可以同時(shí)考慮對(duì)關(guān)節(jié)速度和加速度等的約束。

軌跡規(guī)劃方法有梯形法、正弦法、多項(xiàng)式、多項(xiàng)式樣條和B 樣條等。梯形曲線方式能保證運(yùn)動(dòng)時(shí)間最短，但是其加速度不連續(xù)，不利于運(yùn)動(dòng)和控制的平穩(wěn)性。正弦法計(jì)算簡(jiǎn)單且能保證末端位置速度、加速度連續(xù)，但正弦法執(zhí)行時(shí)間性較差。文獻(xiàn)［1－4］采用正弦法進(jìn)行操作空間軌跡規(guī)劃。多項(xiàng)式插值在高階時(shí)會(huì)產(chǎn)生龍格現(xiàn)象，軌跡誤差增大并且造成運(yùn)動(dòng)控制不平穩(wěn)，所以該方法要求插值點(diǎn)和約束條件不宜過(guò)多，文獻(xiàn)［5］采用3 次多項(xiàng)式擬合方法對(duì)3 自由度機(jī)械手進(jìn)行了軌跡規(guī)劃。多項(xiàng)式樣條和B 樣條避免了龍格現(xiàn)象，而且運(yùn)動(dòng)平穩(wěn)性好，有利于軌跡跟蹤控制，B 樣條方法具有凸包性，有利于對(duì)關(guān)節(jié)約束的解決。文獻(xiàn)［6］采用三次樣條對(duì)機(jī)械手進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃，文獻(xiàn)［7］綜合操作空間3 次樣條規(guī)劃和關(guān)節(jié)空間7 次B 樣條對(duì)機(jī)器人機(jī)械手軌跡規(guī)劃，文獻(xiàn)［8］把3 次樣條和5 次B 樣條兩種軌跡規(guī)劃方法的結(jié)果進(jìn)行分析對(duì)比，結(jié)果顯然5 次B 樣條軌跡規(guī)劃優(yōu)于3 次樣條。

綜上所述，本文作者采用C4連續(xù)的五次B 樣條曲線進(jìn)行基于關(guān)節(jié)空間的軌跡規(guī)劃，利用B 樣條曲線的凸包性將關(guān)節(jié)速度和加速度約束轉(zhuǎn)化為對(duì)曲線控制頂點(diǎn)的約束。利用MATLAB 遺傳算法工具箱對(duì)目標(biāo)函數(shù)進(jìn)行優(yōu)化，找到使目標(biāo)函數(shù)取最小值的軌跡。關(guān)節(jié)的速度和加速度約束限制通過(guò)罰函數(shù)的值來(lái)解決［9－ 10］。

1 軌跡規(guī)劃構(gòu)造與優(yōu)化

1.1 5 次B 樣條曲線構(gòu)造

在笛卡兒空間中構(gòu)造六足機(jī)器人足端的軌跡，確定足端路徑的位置－時(shí)間序列矢量，通過(guò)腿部的運(yùn)動(dòng)學(xué)逆解方程得到對(duì)應(yīng)的關(guān)節(jié)空間中的位置－時(shí)間序列矢量，設(shè)序列個(gè)數(shù)為n+1，其求解公式為:

其中p (t)為關(guān)節(jié)在t 時(shí)刻的位置，di為曲線的控制頂點(diǎn)，Ni，k(t)為第i 段k 次B 樣條曲線基函數(shù)。節(jié)點(diǎn)序列為(t1，t2，…，tn+2k)，為了使曲線的首末控制頂點(diǎn)與通過(guò)節(jié)點(diǎn)序列的首末端點(diǎn)重合，設(shè)定:

ti(i=k+1，k+3，…，k +n +1)為n +1 個(gè)關(guān)節(jié)空間位置－時(shí)間序列矢量對(duì)應(yīng)的時(shí)刻。

為了構(gòu)造B 樣條曲線p(t)需要反求控制頂點(diǎn)di。已知B 樣條曲線經(jīng)過(guò)n+1 個(gè)位置序列，根據(jù)式(1)可以列出n+1 個(gè)對(duì)應(yīng)的方程，而控制頂點(diǎn)個(gè)數(shù)為n+k，所以還需要k－1 個(gè)邊界條件，則可構(gòu)造n + k個(gè)方程求出所有的控制頂點(diǎn)。5 次B 樣條曲線需要4個(gè)邊界條件，若首末端點(diǎn)的速度v0、ve和加速度a0、ve4 個(gè)邊界條件已知，設(shè)

5 次B 樣條曲線n+5 個(gè)方程如下式:

n+k 個(gè)控制頂點(diǎn)可以求出，進(jìn)而可以構(gòu)造5 次B樣條曲線。

1.2 關(guān)節(jié)約束問(wèn)題解決

根據(jù)B 樣條曲線特有性質(zhì):(1)對(duì)B 樣條曲線微分所得曲線還是B 樣條曲線; (2)B 樣條曲線具有凸包性，即曲線全部在控制頂點(diǎn)圍成的凸包內(nèi)。因此，對(duì)關(guān)節(jié)速度和加速度的約束可轉(zhuǎn)化為對(duì)關(guān)節(jié)速度和加速度B 樣條曲線的控制頂點(diǎn)約束。

位置曲線的構(gòu)造如上小節(jié)所述，速度和加速度B樣條曲線的構(gòu)造遵循如下規(guī)律:

分別是速度、加速度和脈動(dòng)B 樣條曲線的控制頂點(diǎn)。

第m 關(guān)節(jié)的速度和加速度的允許最大值分別為vm和Am，為滿足約束要求:

1.3 遺傳算法軌跡優(yōu)化

遺傳算法是一種全局搜索優(yōu)化方法，用計(jì)算機(jī)程序模擬自然界中生物的優(yōu)勝劣汰進(jìn)化現(xiàn)象，通過(guò)選擇，交叉和變異算子來(lái)實(shí)現(xiàn)優(yōu)化，遺傳算法與傳統(tǒng)的優(yōu)化算法相比不容易陷入局部最優(yōu)解。

適應(yīng)度函數(shù)如式(5)，采用時(shí)間總合與罰函數(shù)相加，利用MATLAB 遺傳算法工具箱搜索適應(yīng)度函數(shù)取得最小值時(shí)的時(shí)間節(jié)點(diǎn)序列 (t1，t2，…，tn+2k)。約束問(wèn)題利用罰函數(shù)來(lái)解決。

其中φ(t)為罰函數(shù)，當(dāng)時(shí)間不滿足關(guān)節(jié)約束時(shí)φ(t)取值為1 000，使適應(yīng)度函數(shù)過(guò)大從而在進(jìn)化過(guò)程被淘汰。反之滿足關(guān)節(jié)約束時(shí)φ(t)取值為零。從而不影響適應(yīng)度函數(shù)的排序。

2 仿真

利用MATLAB 遺傳算法工具箱進(jìn)行軌跡規(guī)劃，六足機(jī)器人腿部3 個(gè)關(guān)節(jié)的速度和加速度約束如表1。

表1 腿部各關(guān)節(jié)約束值

時(shí)間節(jié)點(diǎn)序列為

u= ［0 0 0 0 0 0 t7t8t9t10t11t11t11t11t11t11］

足端路徑是橢圓的上半部分，如圖1 所示。并且路徑經(jīng)過(guò)已知笛卡兒空間六點(diǎn)P1、P2、P3、P4、P5、P6，各點(diǎn)分別對(duì)應(yīng)帶優(yōu)化時(shí)間節(jié)點(diǎn)t6、t7、t8、t9、t10、t11。

圖1 足端路徑

由笛卡兒空間的已知點(diǎn)位置序列，利用逆。解方程計(jì)算對(duì)應(yīng)的關(guān)節(jié)空間的途經(jīng)位置序列，如表2。

表2 位置時(shí)間序列

利用MATLAB 遺傳算法工具箱優(yōu)化后得到時(shí)間最優(yōu)時(shí)的位置時(shí)間序列如表2，各關(guān)節(jié)B 樣條軌跡控制點(diǎn)如表3。

表3 各關(guān)節(jié)B 樣條軌跡控制點(diǎn)

規(guī)劃各關(guān)節(jié)時(shí)間最優(yōu)的位移、速度、加速度和脈動(dòng)軌跡如圖2 所示。

圖2 各關(guān)節(jié)時(shí)間最優(yōu)的位移、速度、加速度和脈動(dòng)軌跡圖

由圖2 可以看出，各關(guān)節(jié)時(shí)間最優(yōu)的位移、速度、加速度和脈動(dòng)軌跡均連續(xù)，速度和加速度的最大值在約束范圍內(nèi)。這有利于腿部控制的平穩(wěn)性和提高控制精度，并且有利于延長(zhǎng)控制電機(jī)和六足機(jī)器人使用壽命。

3 結(jié)論

在關(guān)節(jié)空間利用五次B 樣條曲線對(duì)六足仿生機(jī)器人進(jìn)行軌跡規(guī)劃，得到位移、速度、加速度和脈動(dòng)均連續(xù)的軌跡。利用MATLAB 遺傳算法工具箱對(duì)運(yùn)動(dòng)軌跡進(jìn)行時(shí)間優(yōu)化，關(guān)節(jié)約束問(wèn)題通過(guò)罰函數(shù)解決。仿真結(jié)果表明，該軌跡規(guī)劃方法構(gòu)造的軌跡能夠使六足機(jī)器人腿部控制平穩(wěn)、運(yùn)動(dòng)快速。為下一步控制做好準(zhǔn)備。

［1］劉亞軍，黃田．6R 操作臂逆運(yùn)動(dòng)學(xué)分析與軌跡規(guī)劃［J］．機(jī)械工程學(xué)報(bào)，2012，48(3):9－15．

［2］郝登運(yùn)，張國(guó)華，韓荻，等．2 自由度重型搖擺臺(tái)的逆運(yùn)動(dòng)學(xué)分析及其軌跡規(guī)劃［J］．機(jī)械設(shè)計(jì)，2011，28(2):58－60，93．

［3］梅江平，王攀峰，黃田．二平動(dòng)自由度高速并聯(lián)機(jī)械手軌跡規(guī)劃與運(yùn)動(dòng)控制［J］．制造業(yè)自動(dòng)化，2004，26(9):29－32，61．

［4］田桂，謝建，陳永寧，等．導(dǎo)彈起豎過(guò)程軌跡規(guī)劃及跟蹤控制研究［J］．系統(tǒng)仿真學(xué)報(bào)［J］．2012，24(11):2356－2371．

［5］BORYGA M，GRABOS A．Planning of Manipulator Motion Trajectory with Higher-degree Polynomials Use［J］．Mechanism and Machine Theory，2009，44:1400－1419．

［6］GASPARETTO Alessandro，ZANOTTO Vanni．A Technique for Time-jerk Optimal Planning of Robot Trajectories［J］．Robotics and Computer-Integrated Manufacturing，2008，24:415－426．

［7］LIU Huashan，LAI Xiaobo，WU Wenxiang．Time-optimal and Jerk-continuous Trajectory Planning for Robot Manipulators with Kinematic Constraints［J］．Robotics and Computer-Integrated Manufacturing，2013，29:309－317．

［8］GASPARETTO A，ZANOTTO V．Optimal Trajectory Planning for Industrial Robots［J］．Advances in Engineering Software，2010，41:548－556．

［9］曹中一，王鶴，吳萬(wàn)榮，等．噴漿機(jī)械手時(shí)間最優(yōu)與脈動(dòng)最優(yōu)軌跡規(guī)劃［J］．中南大學(xué)學(xué)報(bào)，2013，44(1):114－121．

［10］徐海黎，解祥榮，莊健，等．工業(yè)機(jī)器人的最優(yōu)時(shí)間與最優(yōu)能量軌跡規(guī)劃［J］．機(jī)械工程學(xué)報(bào)，2010，46(9):19－25．