蔡國(guó)慶 郝瑞林 周利杰 石 巖

(1. 河北省工業(yè)機(jī)械手控制與可靠性技術(shù)創(chuàng)新中心，河北 滄州 061001；2. 滄州市工業(yè)機(jī)械手控制與可靠性技術(shù)創(chuàng)新中心，河北 滄州 061001；3. 河北水利電力學(xué)院機(jī)械工程系，河北 滄州 061001)

隨著自動(dòng)化技術(shù)的進(jìn)步，智能制造產(chǎn)業(yè)迅速發(fā)展，工業(yè)機(jī)器人被廣泛應(yīng)用于食品加工生產(chǎn)線中，執(zhí)行搬運(yùn)、碼垛、包裝等作業(yè)任務(wù)[1-3]，且在提高經(jīng)濟(jì)效益、生產(chǎn)效率、產(chǎn)品質(zhì)量及改善工人勞動(dòng)強(qiáng)度等方面發(fā)揮了重要作用[4]。王曦鳴[5]通過(guò)機(jī)器人離線編程軟件對(duì)生產(chǎn)線進(jìn)行了仿真設(shè)計(jì)，完成了工作站的布置工作，但仿真軟件中只有限定參數(shù)的機(jī)器人模型，其與工況不匹配且仿真結(jié)果不夠準(zhǔn)確；楊世明等[6]對(duì)并聯(lián)烹飪機(jī)器人的位置進(jìn)行了分析求解和工作空間的仿真計(jì)算，為后續(xù)軌跡規(guī)劃提供了依據(jù)；王君等[7]利用三次多項(xiàng)式插值的方法對(duì)UR5機(jī)器人進(jìn)行了軌跡規(guī)劃，得到了連續(xù)的軌跡曲線，但未考慮加速度的影響；李琚陳等[8]通過(guò)求解曲面族包絡(luò)得到機(jī)器人的工作空間，并采用了五次多項(xiàng)式插值對(duì)機(jī)器人進(jìn)行軌跡規(guī)劃，得到了關(guān)節(jié)連續(xù)的角度、角速度和角加速度曲線，但未考慮運(yùn)動(dòng)過(guò)程點(diǎn)位的控制；張蕾等[9]采用七次多項(xiàng)式對(duì)4自由度機(jī)器人進(jìn)行運(yùn)動(dòng)規(guī)劃，提高了軌跡精度，但在插值區(qū)邊緣發(fā)生振動(dòng)，出現(xiàn)龍格現(xiàn)象。

基于上述問(wèn)題，研究擬以遨博公司生產(chǎn)的6自由度串聯(lián)機(jī)器人為研究對(duì)象，對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模、驗(yàn)證，采用隨機(jī)抽樣的方法對(duì)機(jī)器人工作空間進(jìn)行仿真求解；提出一種3段五次多項(xiàng)式插值的軌跡規(guī)劃方法，并通過(guò)MATLAB仿真分析驗(yàn)證其有效性，旨在使機(jī)器人在運(yùn)動(dòng)連續(xù)平穩(wěn)的前提下，提高對(duì)軌跡插值點(diǎn)位的追蹤控制能力，為食品搬運(yùn)臺(tái)在生產(chǎn)線的布局提供依據(jù)。

1 機(jī)器人本體結(jié)構(gòu)

食品搬運(yùn)任務(wù)選用遨博公司生產(chǎn)的AUBO-i3型小自重串聯(lián)機(jī)器人，共6個(gè)旋轉(zhuǎn)自由度。其本體結(jié)構(gòu)由機(jī)身、臂部、腕部和手部組成，末端裝置視食品特點(diǎn)可以選擇為吸盤或夾爪(見圖1)，各關(guān)節(jié)運(yùn)動(dòng)范圍均為±170°，最大速度為210(°)/s。

2 運(yùn)動(dòng)學(xué)分析

2.1 運(yùn)動(dòng)學(xué)建模

參照D-H規(guī)則，根據(jù)AUBO-i3機(jī)器人本體結(jié)構(gòu)與尺寸參數(shù)建立連桿坐標(biāo)系(見圖2)，得到機(jī)器人連桿長(zhǎng)度ai-1、連桿轉(zhuǎn)角αi-1、連桿偏距di、關(guān)節(jié)角θi如表1所示。

圖1 機(jī)器人本體結(jié)構(gòu)

圖2 D-H連桿坐標(biāo)系

連桿iai-1/mmαi-1/(°)di/mmθi/(°)關(guān)節(jié)變量100d1θ1θ120-90d2θ2θ23a300θ3θ34a400θ4θ45090d5θ5θ560-90d6θ6θ6

(1)

(2)

式中：

[noa]3×3——機(jī)器人姿態(tài)變換矩陣；

[pxpypz]T——機(jī)器人位置變換矩陣。

逆運(yùn)動(dòng)學(xué)問(wèn)題是求機(jī)器人的逆解，即已知機(jī)器人末端在基坐標(biāo)系中的表達(dá)，求解各關(guān)節(jié)角度，可以通過(guò)式(3)解析求解，限于篇幅限制將不再展開。而逆運(yùn)動(dòng)學(xué)的解與機(jī)器人的位置姿態(tài)有關(guān)，一般存在多解的情況，應(yīng)根據(jù)關(guān)節(jié)角度的運(yùn)動(dòng)范圍及具體條件尋找最優(yōu)解。

(3)

應(yīng)用MATLAB中機(jī)器人工具箱，根據(jù)表1的連桿參數(shù)對(duì)機(jī)器人進(jìn)行示教仿真，結(jié)果如圖3所示。當(dāng)在θ2=-90°、θ4=90°、θ1=θ3=θ5=θ6=0°的配置下，機(jī)器人的位置和姿態(tài)與實(shí)際情況是完全一致的，驗(yàn)證了機(jī)器人運(yùn)動(dòng)學(xué)模型的正確性。

圖3 機(jī)器人示教模型

2.2 工作空間仿真分析

通過(guò)對(duì)機(jī)器人運(yùn)動(dòng)學(xué)建模，得到機(jī)器人運(yùn)動(dòng)學(xué)正解，建立機(jī)器人末端坐標(biāo)相對(duì)于基坐標(biāo)的位置姿態(tài)變換關(guān)系，機(jī)器人工作空間可以根據(jù)變換關(guān)系求解獲得。基于隨機(jī)抽樣的方法，可將θ1、θ2、θ3、θ4、θ5、θ6在其取值范圍內(nèi)隨機(jī)取值，通過(guò)式(2)可得到當(dāng)前關(guān)節(jié)配置下的末端位置。當(dāng)關(guān)節(jié)取樣足夠多時(shí)，可以得到機(jī)器人末端位置的集合，即機(jī)器人的工作空間，其計(jì)算模型表達(dá)為：

(4)

式中：

w——工作空間；

[pxpypz]T——機(jī)器人位置變換矩陣；

θi——關(guān)節(jié)角，rad；

θimin、θimax——關(guān)節(jié)i角度取值的下限與上限。

通過(guò)MATLAB中rand()函數(shù)對(duì)模型編程，每個(gè)關(guān)節(jié)角均隨機(jī)采樣5 000個(gè)點(diǎn)，求解機(jī)器人工作空間，結(jié)果如圖4所示。由圖4可知，機(jī)器人的工作空間近似于一個(gè)原點(diǎn)為(0,0,160)，半徑為r=600 mm的球形空間。機(jī)器人的基座一般固定于工作臺(tái)或者地面，基座下方的區(qū)域是不可到達(dá)的，即待搬運(yùn)的食品位置盡量避免放置在z<0 mm的區(qū)域。在半徑500～600 mm區(qū)域內(nèi)，點(diǎn)位較稀疏，說(shuō)明此空間內(nèi)所對(duì)應(yīng)的關(guān)節(jié)配置少，且該區(qū)域處于工作空間的邊界容易產(chǎn)生奇異配置，使得機(jī)器人控制困難。因此食品位置應(yīng)置于球形工作空間半徑r<500 mm的空間內(nèi)，以保證有優(yōu)選的關(guān)節(jié)配置抓取食品。此外，工作空間計(jì)算模型并未考慮機(jī)器人本體結(jié)構(gòu)，當(dāng)末端處于工作空間中心附近區(qū)域時(shí)，機(jī)器人本體機(jī)構(gòu)之間很可能發(fā)生干涉，因此設(shè)置一個(gè)閾值r>200 mm，避免干涉的發(fā)生。故較優(yōu)的搬運(yùn)臺(tái)布置空間為z>0 mm且200 mm

圖4 機(jī)器人工作空間云圖

3 軌跡規(guī)劃

3.1 軌跡函數(shù)構(gòu)造

軌跡規(guī)劃是為了控制機(jī)器人的運(yùn)動(dòng)過(guò)程，通過(guò)良好的關(guān)節(jié)運(yùn)動(dòng)約束，得到平穩(wěn)的運(yùn)動(dòng)[10]。根據(jù)作業(yè)任務(wù)選擇規(guī)劃方式[11]，當(dāng)進(jìn)行連續(xù)路徑作業(yè)時(shí)，通常采用笛卡爾空間進(jìn)行規(guī)劃，如弧焊、噴涂等[12]。當(dāng)執(zhí)行對(duì)末端軌跡無(wú)特殊要求的任務(wù)時(shí)，往往采用關(guān)節(jié)空間進(jìn)行規(guī)劃，如點(diǎn)焊、搬運(yùn)等[13]。

機(jī)器人執(zhí)行搬運(yùn)任務(wù)時(shí)，運(yùn)動(dòng)過(guò)程中應(yīng)保持平穩(wěn)，避開空間中的障礙。研究提出一種3段五次多項(xiàng)式插值方法，將機(jī)器人的整個(gè)運(yùn)動(dòng)軌跡分為3個(gè)五次多項(xiàng)式部分，一方面保證了運(yùn)動(dòng)的連續(xù)性，另一方面增加了對(duì)軌跡上兩個(gè)過(guò)程點(diǎn)的控制，利于避障和控制運(yùn)動(dòng)過(guò)程。其表達(dá)式為：

(5)

式中：

θi——第i段多項(xiàng)式插值函數(shù)；

aij——第i段多項(xiàng)式第j次項(xiàng)的系數(shù)，為待確定的常量。

其已知條件即可控參數(shù)為3段五次多項(xiàng)式函數(shù)始末點(diǎn)與中間兩個(gè)控制點(diǎn)的時(shí)間、位置、速度及加速度，為了保證3段五次多項(xiàng)式軌跡整體連續(xù)平穩(wěn)，要求每段多項(xiàng)式的連接點(diǎn)處的速度、加速度相等。因此，常量aij可由起始點(diǎn)P1、第1個(gè)過(guò)程控制點(diǎn)P2、第2個(gè)過(guò)程控制點(diǎn)P3和終止點(diǎn)P4獲得：

(6)

(7)

(8)

式中：

ti——軌跡經(jīng)過(guò)Pi點(diǎn)的時(shí)間，s；

θi(ti)——軌跡經(jīng)過(guò)Pi點(diǎn)的角位移，rad；

將aij代入式(5)，得到3段五次多項(xiàng)式的運(yùn)動(dòng)軌跡函數(shù)、速度函數(shù)及加速度函數(shù)。

3.2 仿真分析

在機(jī)器人工作空間中，根據(jù)產(chǎn)線布局及食品搬運(yùn)要求，選取機(jī)器人夾爪運(yùn)動(dòng)中的4個(gè)位置點(diǎn)P1、P2、P3、P4規(guī)劃運(yùn)動(dòng)軌跡，分別代表運(yùn)動(dòng)起始點(diǎn)、抬起點(diǎn)、下降點(diǎn)、終止點(diǎn)，其相關(guān)數(shù)據(jù)參數(shù)見表2。

表2 點(diǎn)位數(shù)據(jù)參數(shù)

根據(jù)3段五次多項(xiàng)式插補(bǔ)算法，對(duì)表2中4個(gè)點(diǎn)位進(jìn)行插補(bǔ)規(guī)劃，考慮食品搬運(yùn)機(jī)器人在啟停階段應(yīng)當(dāng)穩(wěn)定的要求，設(shè)置起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)角速度及角加速度均為0，為便于分析計(jì)算，設(shè)定兩個(gè)控制點(diǎn)P2、P3的角速度及角加速度也為0，運(yùn)動(dòng)總時(shí)間為4 s，第1 s經(jīng)過(guò)P2點(diǎn)，第3 s經(jīng)過(guò)P3點(diǎn)。為便于比較分析，同樣的數(shù)據(jù)采用五次多項(xiàng)式和七次多項(xiàng)式進(jìn)行插補(bǔ)規(guī)劃。前3個(gè)關(guān)節(jié)的角度、角速度及角加速度的運(yùn)行曲線分別見圖5～圖7。

圖5 關(guān)節(jié)角位移—時(shí)間曲線

圖6 關(guān)節(jié)角速度—時(shí)間曲線

圖7 關(guān)節(jié)角加速度—時(shí)間曲線

由圖5可知，3段五次多項(xiàng)式與七次多項(xiàng)式插值均經(jīng)過(guò)了過(guò)程點(diǎn)P2、P3，達(dá)到了期望位置，而五次多項(xiàng)式則偏離了過(guò)程點(diǎn)P2、P3，難以精確地經(jīng)過(guò)既定的過(guò)程點(diǎn)。此外，七次多項(xiàng)式插值雖然能準(zhǔn)確經(jīng)過(guò)P2、P3，但是在運(yùn)動(dòng)的始末階段易產(chǎn)生波動(dòng)及無(wú)效運(yùn)動(dòng)，尤其是在圖5(a)和圖5(b)中的第0～1，3～4 s表現(xiàn)明顯。由圖6可知，3段五次多項(xiàng)式插值方法不僅能夠準(zhǔn)確追蹤過(guò)程點(diǎn)P2、P3的位置，同樣可以準(zhǔn)確控制過(guò)程點(diǎn)P2、P3的速度，在第1，3 s 時(shí)達(dá)到速度為0的要求，而其他兩種插值方法都難以滿足對(duì)過(guò)程點(diǎn)速度控制的要求，并且3段五次多項(xiàng)式插值與五次多項(xiàng)式插值在始末階段對(duì)關(guān)節(jié)角度的控制上要比七次多項(xiàng)式更穩(wěn)定。由圖7可知，3種插值方法均獲得了連續(xù)的加速度曲線，說(shuō)明3種方法都可以獲得平穩(wěn)的運(yùn)動(dòng)軌跡，不會(huì)在運(yùn)動(dòng)過(guò)程中產(chǎn)生振動(dòng)沖擊，但3段五次多項(xiàng)式能準(zhǔn)確控制過(guò)程點(diǎn)P2、P3的加速度。綜上，3種插補(bǔ)算法雖然可以獲得連續(xù)的運(yùn)動(dòng)軌跡，但3段五次多項(xiàng)式的軌跡規(guī)劃方法對(duì)機(jī)器人運(yùn)動(dòng)的過(guò)程控制更具優(yōu)勢(shì)。

3.3 實(shí)驗(yàn)驗(yàn)證

設(shè)定4個(gè)過(guò)程控制點(diǎn)的參數(shù)與仿真條件一致，采用3段五次多項(xiàng)式的插值方法規(guī)劃?rùn)C(jī)器人運(yùn)動(dòng)軌跡，并對(duì)關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3的角度參數(shù)進(jìn)行采樣，采樣間隔0.2 s，采樣數(shù)據(jù)見表3。通過(guò)采樣點(diǎn)數(shù)據(jù)與規(guī)劃曲線對(duì)比發(fā)現(xiàn)，機(jī)器人運(yùn)動(dòng)軌跡與規(guī)劃軌跡是吻合的，驗(yàn)證了仿真結(jié)論。

表3 采樣點(diǎn)數(shù)據(jù)

4 結(jié)論

針對(duì)食品搬運(yùn)機(jī)器人的布局及軌跡規(guī)劃問(wèn)題，根據(jù)AUBO-i3機(jī)器人本體參數(shù)進(jìn)行了運(yùn)動(dòng)學(xué)建模，并通過(guò)MATLAB示教仿真驗(yàn)證了模型的正確性。結(jié)果表明，食品搬運(yùn)臺(tái)的合理布置空間為z>0 mm且200 mm