一種機(jī)器人最優(yōu)加速度軌跡規(guī)劃算法研究

趙 亮， 仲 崇 權(quán)， 鄭 飛 翔

( 1.大連理工大學(xué) 電子信息與電氣工程學(xué)部， 遼寧 大連 116024；2.華為數(shù)字技術(shù)有限公司， 江蘇 蘇州 215000 )

一種機(jī)器人最優(yōu)加速度軌跡規(guī)劃算法研究

趙 亮*1， 仲 崇 權(quán)1， 鄭 飛 翔2

( 1.大連理工大學(xué) 電子信息與電氣工程學(xué)部， 遼寧 大連 116024；2.華為數(shù)字技術(shù)有限公司， 江蘇 蘇州 215000 )

針對機(jī)器人調(diào)試過程中遇到的超限問題，提出了一種滿足運(yùn)動約束的加速度最優(yōu)T型速度曲線控制方法，利用拉格朗日法分析了碼垛機(jī)器人的動力學(xué)性能，論述了機(jī)器人軌跡規(guī)劃算法的實現(xiàn)過程．該算法在保證運(yùn)動平穩(wěn)的基礎(chǔ)上，充分發(fā)揮了驅(qū)動關(guān)節(jié)的驅(qū)動性能．與傳統(tǒng)速度曲線的對比表明，無論單軸還是軸組模式下，機(jī)器人的控制效果均得到了明顯的提高．

碼垛機(jī)器人；動力學(xué)；軌跡規(guī)劃；最優(yōu)加速度

0 引 言

機(jī)器人最優(yōu)軌跡規(guī)劃，即根據(jù)給定的路徑點(diǎn)規(guī)劃出通過這些點(diǎn)并滿足邊界約束條件的光滑的最優(yōu)運(yùn)動軌跡，是工業(yè)機(jī)器人最優(yōu)控制問題之一[1-2]．早期的軌跡規(guī)劃研究多見于為了最大化操作速度從而減少最小化機(jī)器人總的動作時間[3-4]，然而實際上，能量最優(yōu)也是工業(yè)應(yīng)用中很重要的性能指標(biāo)．關(guān)節(jié)空間軌跡規(guī)劃對機(jī)器人運(yùn)動過程中的軌跡沒有特別嚴(yán)格要求，只規(guī)定初末兩點(diǎn)的位姿[5]．在對機(jī)器人進(jìn)行運(yùn)動控制時，各個位置約束、速度約束、加速度約束是需要考慮的關(guān)鍵問題．當(dāng)機(jī)器人運(yùn)動速度較快時，驅(qū)動關(guān)節(jié)速度和加速度比較容易超限，導(dǎo)致驅(qū)動電流過大、位置偏差過大、超出限位等問題．因此，在對機(jī)器人進(jìn)行軌跡規(guī)劃時必須綜合考慮速度和加速度等約束條件．

張豐華等在考慮運(yùn)動約束的前提下，采用“4-3-4”法利用不同低階多項式對各軌跡段進(jìn)行插補(bǔ)，以實現(xiàn)流暢連續(xù)的自動碼放貨物的要求[6]．徐海黎等在考慮運(yùn)動約束前提下，以機(jī)器人運(yùn)動過程中的總時間最優(yōu)為目標(biāo)，通過三次多項式插值進(jìn)行型值點(diǎn)連接，使用加權(quán)函數(shù)法定義代價函數(shù)，研究其運(yùn)動過程[1]．Saravanan等在考慮運(yùn)動約束的前提下，使用NURBS曲線規(guī)劃機(jī)器人運(yùn)動軌跡，通過優(yōu)化時間來提高機(jī)器人的工作效率[7]．Piazzi等在研究機(jī)器人運(yùn)動過程中討論了其沖擊最小問題，在對機(jī)器人進(jìn)行軌跡規(guī)劃時，提高了機(jī)器人動力學(xué)性能[8]．Zlajpah等精確地建立了機(jī)器人動力學(xué)方程，獲得了滿足運(yùn)動約束的時間最優(yōu)的運(yùn)動軌跡，并在實際應(yīng)用中驗證了其可行性與正確性[9]．

通過合理的軌跡規(guī)劃可以使機(jī)器人運(yùn)動過程中某項性能最優(yōu)[10-12]，本文以四自由度混聯(lián)碼垛機(jī)器人為研究對象，根據(jù)實際對象建立該機(jī)器人的運(yùn)動學(xué)模型，研究其運(yùn)動特性，分析其動力學(xué)性能，提出一種易于滿足運(yùn)動約束的最優(yōu)加速度軌跡規(guī)劃算法，以在保證機(jī)器人運(yùn)動不超限的前提下，充分發(fā)揮驅(qū)動關(guān)節(jié)的驅(qū)動性能．

1 碼垛機(jī)器人動力學(xué)模型

碼垛機(jī)器人的動力學(xué)性能與各個桿件的加速度、桿件的質(zhì)量分布、桿件的轉(zhuǎn)動慣量以及機(jī)構(gòu)的負(fù)載密切相關(guān)．為了提高機(jī)構(gòu)的響應(yīng)速度和可靠性，研究其動力學(xué)性能，對機(jī)器人進(jìn)行動力學(xué)分析已經(jīng)成為研究工作和設(shè)計的重點(diǎn)[13-14]．在碼垛機(jī)器人的實際調(diào)試過程中，加速度的大小對機(jī)器人的控制效果影響較大，所以尋找一段最優(yōu)加速度軌跡具有重要意義．

針對碼垛機(jī)器人含有局部閉鏈的平行四連桿的機(jī)械結(jié)構(gòu)，選取拉格朗日法建立碼垛機(jī)器人的動力學(xué)方程，如圖1所示為碼垛機(jī)器人臂部結(jié)構(gòu)的運(yùn)動簡圖，大臂轉(zhuǎn)動的角度為α，小臂轉(zhuǎn)動的角度為β，作為系統(tǒng)的廣義坐標(biāo)，可描述該二自由度系統(tǒng)．桿件CE、桿件BC、桿件AD、桿件DF的長度分別為lb、lc、la+lb、lc+ld．對于n個連桿組成的機(jī)器人，由拉格朗日函數(shù)描述的系統(tǒng)動力學(xué)方程為

Qi=ddt(?L?q.i)-?L?qi

(1)

式中：L為拉格朗日函數(shù)；Qi表示作用在第i個關(guān)節(jié)的廣義驅(qū)動力矩，i=1,2,…,n；qi(i=1,2,…,n)為使系統(tǒng)具有完全確定位置的廣義關(guān)節(jié)變量；

q.

i為廣義坐標(biāo)qi對時間t的一階導(dǎo)數(shù)，即相應(yīng)的廣義關(guān)節(jié)速度．

圖1 碼垛機(jī)器人臂部結(jié)構(gòu)

建立廣義坐標(biāo)α、β與兩個移動關(guān)節(jié)在基坐標(biāo)系的位置關(guān)系．以腰部某點(diǎn)建立基坐標(biāo)系，機(jī)身某點(diǎn)建立參考坐標(biāo)系O-xyz，豎直移動滑塊的位置坐標(biāo)為(0,za)、水平移動滑塊的位置坐標(biāo)為(xc,0)．桿件BC長度為lc，桿件AB長度為la，根據(jù)分析可知

(2)

(3)

可以得到系統(tǒng)的動力學(xué)方程為

(4)

即

(5)

式中：D(q)為慣性矩陣，h(q,q..)為向心力和哥氏力矢量，G(q)為重力矢量．而向心力和哥氏力只對機(jī)器人高速運(yùn)動有影響，在機(jī)器人低速運(yùn)行時可以忽略．

2 約束分析

2.1 運(yùn)動約束分析

碼垛機(jī)器人的運(yùn)動約束主要包括位移約束、速度約束和加速度約束．在一般運(yùn)動情況下，只要保證位移不超限即可，這對軌跡規(guī)劃的影響很小．這里主要考慮速度約束以及加速度約束，選取T型速度曲線進(jìn)行運(yùn)動約束分析．可以用下列不等式表示為

θ.(t)≤vmaxθ..(t)≤amax

(6)

圖2 速度約束的表示

圖2的運(yùn)動過程分為3個階段：0～t1時刻為加速度為a的勻加速運(yùn)動，t1～t2時刻為勻速運(yùn)動，t2～tf時刻為減速度為a的勻減速運(yùn)動．設(shè)初始時刻位置為θ0，可以建立運(yùn)動學(xué)方程如下：

當(dāng)0≤t

θ..(t)=aθ.(t)=v0+at θ(t)=θ0+v0t+12at2

當(dāng)t1≤t

θ..(t)=0θ.(t)=v0+at1 θ(t)=θ0+(v0+at1)t-12at21

當(dāng)t2≤t

θ..(t)=-aθ.(t)=v0+at1-a(t-t2) θ(t)=θ0-12a(t21+t22+t2)+(v0+at1+at2)t

在終點(diǎn)時刻tf的位移為θf，可以得到關(guān)系式

(7)

在終點(diǎn)時刻tf的速度為vf，可以得到關(guān)系式

vf=v0+at1-a(tf-t2)

(8)

在相同的位移、相同的時間內(nèi)，加速度最小的軌跡驅(qū)動機(jī)構(gòu)的驅(qū)動性能最優(yōu)．當(dāng)t1=t2時，整段軌跡只有加速段和減速段，此時求解的加速度唯一且為最小，非常利于運(yùn)動的平穩(wěn)性以及軌跡跟蹤精度．但是，在具有約束的情況下，此時的速度很容易超過約束范圍．所以，理想的軌跡規(guī)劃應(yīng)當(dāng)是在滿足約束的前提下，盡量采用較小的加速度．

2.2 運(yùn)動約束處理

在相同的位移θf-θ0、相同的時間tf內(nèi)，在沒有任何約束的條件下，當(dāng)t1=t2時，機(jī)器人的運(yùn)動過程沒有勻速段，機(jī)器人的加速度最小，機(jī)器人驅(qū)動性能最優(yōu)．

圖3 加速度約束

在式(7)、(8)中令t1=t2，得到

(9)

令θ0-θf+v0tf=c，由式(9)得，

(10)

當(dāng)vf=v0時，得

(11)

當(dāng)vf≠v0時，令

(12)

可以得到

(13)

當(dāng)式(13)中的t1、t2滿足0≤t1=t2≤tf時才是正確的解，此時的加速度最優(yōu)，其速度曲線如圖4所示．

根據(jù)運(yùn)動約束判斷加速度a是否超過運(yùn)動約束，若超過，則說明該驅(qū)動機(jī)構(gòu)的驅(qū)動性能滿足不了運(yùn)動約束要求；若不超過，此時的加速度a以及相應(yīng)的時間節(jié)點(diǎn)t1、t2構(gòu)成的軌跡段，就是要求的滿足運(yùn)動約束的最優(yōu)加速度軌跡．

圖4 加速度約束下最優(yōu)加速度圖

圖5 速度與加速度約束

由式(13)得到的是滿足加速度約束的運(yùn)動軌跡，這里在t達(dá)到一定程度后，速度v超過了最大速度vmax，運(yùn)動過程中速度最大的階段是在t1～t2的勻速運(yùn)動階段，此時的速度大小為vc=v0+at1．增大加速度a可以降低勻速運(yùn)動階段的速度vc，但是又不能超過最大加速度的限制，所以此時的最優(yōu)加速度-速度曲線應(yīng)如圖6中的B1D1D2B3所示．

根據(jù)式(7)、(8)，可以得到以下方程組：

(14)

解得

(15)

若t1、t2滿足0≤t1≤t2

圖6 速度與加速度約束下最優(yōu)加速度圖

3 性能測試

3.1 試驗系統(tǒng)設(shè)計

如圖7所示，碼垛機(jī)器人的控制系統(tǒng)包括物料傳送帶的傳動控制、機(jī)器人保護(hù)監(jiān)測控制、回原點(diǎn)的運(yùn)動控制以及碼垛機(jī)器人的伺服控制．其中主控器采用大連理工計算機(jī)控制工程有限公司的四軸機(jī)器人運(yùn)動控制器MAC1600，具有1路10/100M以太網(wǎng)通信接口、2路RS-485，支持單軸、多軸、軸組PLC_Open標(biāo)準(zhǔn)指令，支持直線、圓弧插補(bǔ)、多軸同步等多種運(yùn)動控制算法，以高速脈沖的方式實現(xiàn)對驅(qū)動器進(jìn)行精確速度控制、位置控制等功能，滿足對碼垛機(jī)器人控制的需求．采用PLC_Config軟件對MAC1600進(jìn)行梯形圖編程完成系統(tǒng)運(yùn)行中的邏輯控制，包括數(shù)據(jù)采集、外部I/O量的處理、伺服電機(jī)驅(qū)動、運(yùn)動過程及運(yùn)動軌跡的規(guī)劃等．控制系統(tǒng)的電機(jī)選型如表1所示．

圖7 總體控制方案

表1 伺服電機(jī)型號表

3.2 算法的單軸控制驗證

調(diào)用運(yùn)動控制指令中的單軸絕對定位指令MC_MoveAbsolute，該指令控制軸運(yùn)動到指定的目標(biāo)位置．控制機(jī)器人水平軸單軸運(yùn)動，利用伺服器的RS-485通信功能，監(jiān)控水平軸伺服驅(qū)動器的監(jiān)控參數(shù)P0-01，該參數(shù)顯示的是驅(qū)動器的輸出轉(zhuǎn)矩相對于額定轉(zhuǎn)矩的百分比．單軸控制機(jī)器人分別在傳統(tǒng)的T型、S型、最優(yōu)加速度T型3種速度曲線下以相同的位姿運(yùn)行相同的路程，得到3種情況下的力矩曲線如圖8所示．橫坐標(biāo)表示時間，縱坐標(biāo)表示力矩百分比．

圖8 水平軸三種速度曲線下的輸出力矩

圖8中，紅色曲線是機(jī)器人在最優(yōu)加速度T型速度曲線控制下的驅(qū)動關(guān)節(jié)的力矩曲線，其運(yùn)動過程中的力矩峰值相較于其他力矩曲線偏低．又由于其運(yùn)動總時間相同，運(yùn)動總位移相同，運(yùn)動過程中姿態(tài)都相同，所以可以得到結(jié)論：在單軸控制中，最優(yōu)加速度T型速度曲線控制下的驅(qū)動關(guān)節(jié)的驅(qū)動力矩相比傳統(tǒng)的T型、S型速度曲線，在保證運(yùn)動約束的同時，更能夠發(fā)揮驅(qū)動關(guān)節(jié)的驅(qū)動性能．

3.3 算法的軸組控制驗證

調(diào)用連續(xù)曲線運(yùn)動控制指令，控制碼垛機(jī)器人在8 s內(nèi)完成一個抓貨放貨的標(biāo)準(zhǔn)門字型軌跡，其末端運(yùn)動軌跡如圖9所示．

圖9 機(jī)器人末端動作圖

配置封裝好的MC_ContinuousCurve功能塊中的引腳PointsArray為(2 100，0，-240，0)、(1 900，0，240，0)、(0，1 900，240，0)、(0，2 100，-240，0)4個型值點(diǎn)，分別表示抓貨點(diǎn)、第一個途經(jīng)點(diǎn)、第二個途經(jīng)點(diǎn)以及放貨點(diǎn)，配置運(yùn)動總時間為8 s，利用伺服器的RS-485通信功能，同時分別監(jiān)控腰部、水平、豎直軸伺服驅(qū)動器的力矩參數(shù)P0-01，采集數(shù)據(jù)，得到3種情況下的各個驅(qū)動軸的力矩曲線對比圖，如圖10～12所示．橫坐標(biāo)表示時間，縱坐標(biāo)表示力矩百分比．

圖10 軸組腰部軸力矩曲線對比

圖中紅色曲線是機(jī)器人在最優(yōu)加速度T型速度曲線控制下的驅(qū)動關(guān)節(jié)的力矩曲線，其運(yùn)動過程中的力矩峰值相較于其他力矩曲線偏低．又由于其運(yùn)動總時間相同，運(yùn)動總位移相同，運(yùn)動過程中姿態(tài)都相同，所以可以得到結(jié)論：在軸組控制中，最優(yōu)加速度T型速度曲線控制下的驅(qū)動關(guān)節(jié)的驅(qū)動力矩相比傳統(tǒng)的T型、S型速度曲線，在保證運(yùn)動約束的同時，更能夠發(fā)揮驅(qū)動關(guān)節(jié)的驅(qū)動性能．

圖11 軸組水平軸力矩曲線對比

圖12 軸組豎直軸力矩曲線對比

4 結(jié) 語

本文以碼垛機(jī)器人為研究對象，重點(diǎn)研究了機(jī)器人調(diào)試過程中遇到的運(yùn)動超限問題，提出了一種加速度最優(yōu)的、滿足運(yùn)動約束的T型速度曲線控制方法，在保證運(yùn)動平穩(wěn)的基礎(chǔ)上，充分發(fā)揮驅(qū)動關(guān)節(jié)的驅(qū)動性能．詳細(xì)介紹了機(jī)器人軌跡規(guī)劃算法的原理及實現(xiàn)過程，最后，通過與傳統(tǒng)速度曲線下的運(yùn)動進(jìn)行比較，證明無論單軸還是軸組，該最優(yōu)加速度軌跡規(guī)劃算法下的各個驅(qū)動關(guān)節(jié)的驅(qū)動性能最優(yōu)．目前對碼垛機(jī)器人的控制多數(shù)采用的仍是位置控制，接下來打算對通過力矩控制機(jī)器人運(yùn)動的控制方法進(jìn)行深入研究．

[1] 徐海黎,解祥榮,莊 健,等. 工業(yè)機(jī)器人的最優(yōu)時間與最優(yōu)能量軌跡規(guī)劃[J]. 機(jī)械工程學(xué)報, 2010, 46(9):19-25.

XU Haili, XIE Xiangrong, ZHUANG Jian,etal. Global time-energy optimal planning of industrial robot trajectories [J]. Journal of Mechanical Engineering, 2010, 46(9):19-25. (in Chinese)

[2] 叢 明,熊永康,劉 冬,等. 一種并聯(lián)機(jī)械手操作空間最優(yōu)時間軌跡規(guī)劃方法[J]. 機(jī)械設(shè)計, 2016, 33(2):7-13.

CONG Ming, XIONG Yongkang, LIU Dong,etal. A method of time-optimal trajectories planning for parallel robot operation space [J]. Journal of Machine Design, 2016, 33(2):7-13. (in Chinese)

[3] LIN Chunshin, CHANG Porong, LUH J Y S. Formulation and optimization of cubic polynomial joint trajectories for industrial robots [J]. IEEE Transactions on Automatic Control, 1983, 28(12):1066-1074.

[4] GASPARETTO A, ZANOTTO V. A technique for time-jerk optimal planning of robot trajectories [J]. Robotics and Computer-Integrated Manufacturing, 2008, 24(3):415-426.

[5] 徐鵬飛,羅慶生,韓寶玲,等. 新型工業(yè)碼垛機(jī)器人軌跡規(guī)劃研究[J]. 組合機(jī)床與自動化加工技術(shù), 2012(5):70-73,77.

XU Pengfei, LUO Qingsheng, HAN Baoling,etal. Research on trajectory planning of new industrial palletizing robot [J]. Modular Machine Tool & Automatic Manufacturing Technique, 2012(5):70-73,77. (in Chinese)

[6] 張豐華,韓寶玲,羅慶生,等. 基于PLC的新型工業(yè)碼垛機(jī)器人控制系統(tǒng)設(shè)計[J]. 計算機(jī)測量與控制, 2009, 17(11):2191-2193, 2196.

ZHANG Fenghua, HAN Baoling, LUO Qingsheng,etal. Control system design of palletizing robot based on PLC technology [J]. Computer Measurement & Control, 2009, 17(11):2191-2193, 2196. (in Chinese)

[7] SARAVANAN R, RAMABALAN S, BALAMURUGAN C. Multiobjective trajectory planner for industrial robots with payload constraints [J]. Robotica, 2008, 26(6):753-765.

[8] PIAZZI A, VISIOLI A. Global minimum-jerk trajectory planning of robot manipulators [J]. IEEE Transactions on Industrial Electronics, 2000, 47(1):140-149.

[9] ZLAJPAH L, NEMEC B. Implementation of time-optimal path-tracking control on palletizing robots[J]. IEEE International Symposium on Industrial Electronics, 1999, 2:861-866.

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

[11] 張 斌. 基于多約束的機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃[J]. 機(jī)械工程學(xué)報, 2011, 47(21):1-6.

ZHANG Bin. Joint-space trajectory planning for robots under multiple constraints [J]. Journal of Mechanical Engineering, 2011, 47(21):1-6. (in Chinese)

[12] FAHHAM H R, FARID M, KHOORAN M. Time optimal trajectory tracking of redundant planar cable-suspended robots considering both tension and velocity constraints [J]. Journal of Dynamic Systems, Measurement and Control, Transactions of the ASME, 2011, 133(1):011004.

[13] PIRES J N, PAULO S. Robotic de-palletizing system for the non-flat ceramic industry [J]. Industrial Robot, 2003, 30(2):152-158.

[14] PRASSE C, SKIBINSKI S, WEICHERT F,etal. Concept of automated load detection for de-palletizing using depth images and RFID data [C]// Proceedings — 2011 IEEE International Conference on Control System, Computing and Engineering, ICCSCE 2011. Piscataway:IEEE Computer Society, 2011:249-254.

Research on robot trajectory planning algorithm with optimal acceleration

ZHAO Liang*1, ZHONG Chongquan1, ZHENG Feixiang2

( 1.Faculty of Electronic Information and Electrical Engineering, Dalian University of Technology, Dalian 116024, China；2.HUAWEI Digital Technology Co., Ltd., Suzhou 215000, China )

In order to solve the problem of overrun in the process of robot debugging, a control method with optimal acceleration is proposed based on T type velocity curve under motion constraint. The dynamics performance of the palletizing robot is analyzed by using Lagrange method and the realization process of robot trajectory planning algorithm is discussed. This algorithm makes full use of the driving performance of the driving joint on the basis of smooth and stable movements. Compared with the traditional velocity curves, the control effect of the robot is significantly improved both in the single axis mode and the axis group mode.

palletizing robot; dynamics; trajectory planning; optimal acceleration

2016-07-31；

2017-02-16．

國家自然科學(xué)基金資助項目(61472062)；中央高校基本科研業(yè)務(wù)費(fèi)專項資金資助項目(DUT15ZD230,DUT16RC(4)20)；國家科技支撐計劃資助項目(2015BAF20B02).

趙 亮*(1983-)，男，博士，講師， E-mail:zliang@dlut.edu.cn.

1000-8608(2017)02-0177-07

TH16；TP242

A

10.7511/dllgxb201702010