基于閉環(huán)和前饋控制的高速食品分揀機(jī)器人控制技術(shù)

柳振宇

薛毓強(qiáng)2

謝祖強(qiáng)3

(1. 福州第二技師學(xué)院，福建 福州 350000；2. 福州大學(xué)，福建 福州 350000；3. 福建船政交通職業(yè)學(xué)院，福建 福州 350000)

在食品和藥品等輕工業(yè)中，通常需要高速完成諸如包裝和分揀等操作，其操作對(duì)象具有重量輕、體積小等特點(diǎn)[1]。高速并聯(lián)分揀機(jī)器人具有剛度質(zhì)量比大、無(wú)累積誤差、移動(dòng)速度高等優(yōu)點(diǎn)，被廣泛用于食品和制藥等輕工業(yè)[2]。

目前，有關(guān)食品分揀機(jī)器人的相關(guān)技術(shù)研究較多，但對(duì)高速并聯(lián)食品分揀機(jī)器人的研究較少。史亞貝[3]提出了一種基于DSP的三自由度分揀機(jī)器人控制系統(tǒng)，從硬件和軟件兩方面構(gòu)建了一個(gè)系統(tǒng)平臺(tái)。結(jié)果表明，在控制系統(tǒng)的驅(qū)動(dòng)控制下，分揀機(jī)器人可以準(zhǔn)確地從起點(diǎn)移動(dòng)到終點(diǎn)，軌跡相對(duì)平滑，可以達(dá)到預(yù)期的目標(biāo)。伍經(jīng)紋等[4]提出了一種基于ADAMS的三自由度增量機(jī)械手運(yùn)動(dòng)學(xué)分析方法，建立了機(jī)械手的數(shù)學(xué)模型，使用改進(jìn)的修正梯形加速度曲線來(lái)消除運(yùn)動(dòng)始末端對(duì)機(jī)械手的影響。朱向楠等[5]提出了一種基于模糊PID+前饋控制的并聯(lián)機(jī)器人控制方法，并通過(guò)仿真驗(yàn)證了其運(yùn)動(dòng)位移和角位移誤差。結(jié)果表明，改進(jìn)后的控制方法在二自由度并聯(lián)機(jī)器人的運(yùn)動(dòng)位移和角位移跟蹤誤差較小，提高了并聯(lián)機(jī)器人的跟蹤精度。張皓宇等[6]對(duì)并行機(jī)器人的運(yùn)動(dòng)控制和NURBS軌跡進(jìn)行了研究，使用解析方法解決了并行機(jī)器人的逆運(yùn)動(dòng)學(xué)問(wèn)題，并提出了一種基于多層感知器進(jìn)行反向傳播學(xué)習(xí)的神經(jīng)網(wǎng)絡(luò)實(shí)時(shí)求解正運(yùn)動(dòng)學(xué)問(wèn)題。結(jié)果表明，在迭代次數(shù)和執(zhí)行時(shí)間較少的情況下，位置和方向參數(shù)的精度均為0.01 mm。但是，上述研究尚未對(duì)高速并行分揀機(jī)器人的控制方法進(jìn)行詳細(xì)研究，并且未考慮諸如外部干擾等不確定因素對(duì)系統(tǒng)的影響，有一定的局限性，需不斷改進(jìn)和完善。

針對(duì)高速并聯(lián)食品分揀機(jī)器人的控制方法，文章擬提出一種將傳統(tǒng)運(yùn)動(dòng)學(xué)閉環(huán)控制和力矩前饋控制相結(jié)合的高速并聯(lián)食品分揀機(jī)器人的控制方法，在原有遺傳算法整定PID控制參數(shù)的基礎(chǔ)上，引入力矩前饋控制方法進(jìn)行控制，并進(jìn)行驗(yàn)證實(shí)驗(yàn)，旨在為研究高速并聯(lián)食品分揀機(jī)器人的控制方法提供依據(jù)。

1 系統(tǒng)概述

圖1為高速并聯(lián)食品分揀機(jī)器人的體系結(jié)構(gòu)，主要由上位機(jī)系統(tǒng)和下位機(jī)系統(tǒng)兩個(gè)部分組成[7]。上位機(jī)系統(tǒng)是集成到工業(yè)計(jì)算機(jī)平臺(tái)中的一組軟件系統(tǒng)，其主要功能包括可視化、模型求解、視覺(jué)處理、數(shù)據(jù)集成處理以及與下位機(jī)的信息交互[8]。下位機(jī)系統(tǒng)(即以主控制芯片為核心的嵌入式軟件和硬件系統(tǒng))主要提供控制算法集成、伺服電機(jī)閉環(huán)控制、實(shí)時(shí)監(jiān)控和與上位機(jī)的實(shí)時(shí)數(shù)據(jù)交互等功能。

圖2為高速并聯(lián)食品分揀機(jī)器人的本體，由靜平臺(tái)、動(dòng)平臺(tái)、主動(dòng)臂和從動(dòng)臂4部分組成[9]。靜平臺(tái)配備有通過(guò)法蘭連接的伺服電機(jī)和減速器，從動(dòng)臂通過(guò)球形鉸鏈連接到動(dòng)平臺(tái)，每個(gè)分支鏈包含一個(gè)主動(dòng)臂和一個(gè)從動(dòng)臂，動(dòng)平臺(tái)由主、輔平臺(tái)組成[10]。

2 建模與控制方法

高速并行分揀機(jī)器人機(jī)械部分建模分為運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)兩個(gè)部分[11]。運(yùn)動(dòng)學(xué)分析，尤其是逆解分析，是對(duì)分揀機(jī)器人進(jìn)行控制的基礎(chǔ)。動(dòng)態(tài)分析是求解關(guān)節(jié)力矩和實(shí)現(xiàn)動(dòng)態(tài)控制的基礎(chǔ)。

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

分揀機(jī)器人的動(dòng)平臺(tái)只進(jìn)行平移運(yùn)動(dòng)，且各支鏈從動(dòng)臂的運(yùn)動(dòng)相同，因此可以將研究轉(zhuǎn)化為分揀機(jī)器人結(jié)構(gòu)簡(jiǎn)圖(見(jiàn)圖3)。

在靜平臺(tái)中心建立參考坐標(biāo)系O-xyz，將動(dòng)平臺(tái)看成質(zhì)點(diǎn)P1和P2，則主(輔)動(dòng)平臺(tái)參考點(diǎn)P1(P2)的位置矢量r=(xyz)T如式(1)所示[12]。

(1)

式中：

ei——從O到Ai的向量；

l1——主動(dòng)臂桿長(zhǎng)，mm；

ui——單位矢量(沿主動(dòng)臂)；

l2——從動(dòng)臂桿長(zhǎng)，mm；

wi——單位矢量(沿從動(dòng)臂)；

s——P1到P2的距離,mm;

根據(jù)機(jī)構(gòu)的裝配模式，主動(dòng)臂的轉(zhuǎn)角θi整理如式(2)所示[13]。

(2)

式中：

圖1 系統(tǒng)結(jié)構(gòu)

圖3 分揀機(jī)器人結(jié)構(gòu)簡(jiǎn)圖

根據(jù)式(2)求解θi后，wi的值如式(3)所示[14]：

(3)

將式(1) 關(guān)于時(shí)間求一次導(dǎo)數(shù)得到速度模型如式(4)所示[15]。

Jqθg=Jxxg，

(4)

式中：

J——雅克比矩陣。

將式(1)關(guān)于時(shí)間求二次導(dǎo)數(shù)得到角加速度模型如式(5)所示[16]。

θgg=Jxgg+f(xg)，

(5)

式中：

f=(f1f2f3f4)T；

2.2 動(dòng)力學(xué)模型

在完成剛體動(dòng)力學(xué)建模前，應(yīng)將整個(gè)食品分揀機(jī)器人視為剛性的，忽略分揀機(jī)器人的摩擦，并且應(yīng)將分揀機(jī)器人的主臂和從屬臂視為勻質(zhì)桿件[17]。

參考食品分揀機(jī)器人的結(jié)構(gòu)圖(圖3)，在第i個(gè)支鏈的從動(dòng)臂鏈接上建立坐標(biāo)系Obi-xbiybizbi，其中，原點(diǎn)Obi在分揀機(jī)器人的每個(gè)支鏈的質(zhì)心上，zbi軸的正方向是連桿軸線的方向，垂直于xbi軸，且在wi與wi×ui平面中，根據(jù)右手定則確定ybi軸方向。并依據(jù)虛功原理，如式(6)所示[18]。

(6)

式中：

τ——主動(dòng)關(guān)節(jié)驅(qū)動(dòng)轉(zhuǎn)矩，τ=(τ1τ2τ3τ4)T；

rgg——?jiǎng)悠脚_(tái)質(zhì)心加速度，rgg=(xggyggzgg)T；

m1、m2——主平臺(tái)和輔平臺(tái)質(zhì)量，kg；

IA——主動(dòng)臂折算到轉(zhuǎn)軸的轉(zhuǎn)動(dòng)慣量，kg·m2；

τAg——主動(dòng)臂關(guān)于轉(zhuǎn)軸的重力矩，N·m；

Grod——從動(dòng)臂連桿的重力，N；

Arod——從動(dòng)臂連桿的慣性力，N；

Mrod——從動(dòng)臂連桿的慣性力矩，N·m。

將虛位移δθ=Jδr代入式(6)，可得食品分揀機(jī)器人剛體動(dòng)力學(xué)模型如式(7)所示。

τ=τA+τp+τrod，

(7)

式中：

τA、τrod、τp——作用在主動(dòng)臂、從動(dòng)臂、動(dòng)平臺(tái)與負(fù)載上的力與力矩；

τA=IAθgg+τAg；

τp=J-T(margg+mgg)。

2.3 控制方法

確保高速并聯(lián)食品分揀機(jī)器人準(zhǔn)確運(yùn)行的最基本方法是制定控制方法，控制中需要解決的是建模和參數(shù)的不確定性[19]。建模不確定性無(wú)法排除，只能不斷提高控制器的魯棒性，而參數(shù)的不確定性可以通過(guò)自整定來(lái)克服。

為了提高高速并聯(lián)分揀機(jī)器人分揀的快速性和抗干擾能力，提出一種基于遺傳算法自整定的PID+前饋運(yùn)動(dòng)控制方法，即在原有遺傳算法整定PID控制參數(shù)的基礎(chǔ)上，引入力矩前饋控制方法進(jìn)行動(dòng)態(tài)控制，與反饋控制一起作用至電機(jī)軸上[20]。

通過(guò)遺傳算法對(duì)食品分揀機(jī)器人的PID參數(shù)進(jìn)行整定時(shí)，有必要建立評(píng)價(jià)指標(biāo)，以確保調(diào)整過(guò)程的合理性和準(zhǔn)確性。文中，時(shí)間與絕對(duì)誤差乘積的積分作為評(píng)價(jià)指標(biāo)，即：

(8)

表1為遺傳算法進(jìn)行整定后，速度環(huán)和位置環(huán)PID參數(shù)的最優(yōu)解。

高速并聯(lián)食品分揀機(jī)器人的實(shí)際操作狀態(tài)通常是高速且頻繁的加/減速，會(huì)導(dǎo)致多支鏈相互作用和慣性力(如科氏力、向心力)的干擾越大，傳統(tǒng)的PID控制無(wú)法滿足系統(tǒng)的快速響應(yīng)[22]。因此，在傳統(tǒng)運(yùn)動(dòng)學(xué)閉環(huán)控制的基礎(chǔ)上，引入力矩前饋控制方法進(jìn)行補(bǔ)償，抵消分支鏈耦合和動(dòng)態(tài)力對(duì)系統(tǒng)控制的部分影響。

運(yùn)動(dòng)學(xué)閉環(huán)控制系統(tǒng)雖然具有一定的抗干擾功能，但無(wú)法保證高速分揀機(jī)器人控制系統(tǒng)的準(zhǔn)確性，引入力矩前饋控制方法后，系統(tǒng)可以實(shí)時(shí)求解變量的動(dòng)態(tài)模型，獲得期望力矩并執(zhí)行力矩前饋補(bǔ)償。由于電流環(huán)采取封裝形式，通常不對(duì)用戶開(kāi)放。因此，力矩前饋補(bǔ)償?shù)淖饔命c(diǎn)基本會(huì)在電流環(huán)路之前。相對(duì)而言，力矩前饋具有系統(tǒng)性和互補(bǔ)性優(yōu)勢(shì)，能有效改善系統(tǒng)動(dòng)態(tài)性能。建模誤差和摩擦的影響由可閉環(huán)控制消除。圖4為力矩前饋控制框圖。

表1 最優(yōu)參數(shù)

其原理是：使用先前建立的動(dòng)力學(xué)模型來(lái)計(jì)算操作期間分揀機(jī)器人的位置和方向的變化，以獲得此時(shí)的期望力矩。通過(guò)分揀機(jī)器人的單鏈力矩前饋控制器，所需力矩被轉(zhuǎn)換為電信號(hào)，該信號(hào)作用在電流回路上，以實(shí)時(shí)抑制高速動(dòng)態(tài)干擾。通常情況下，分揀機(jī)器人的運(yùn)動(dòng)軌跡是相對(duì)固定的，可以使分揀機(jī)器人的各種狀態(tài)進(jìn)行離線求解，并通過(guò)查表來(lái)獲得驅(qū)動(dòng)扭矩，從而大大減少在線操作量。力矩前饋控制環(huán)節(jié)的傳遞函數(shù)GTF為：

(9)

3 結(jié)果與分析

3.1 試驗(yàn)參數(shù)

為了對(duì)所提出的控制方法進(jìn)行驗(yàn)證，將所提出的控制方法與常規(guī)PID控制方法和常規(guī)PID+前饋控制方法進(jìn)行比較。為了確保試驗(yàn)結(jié)果的準(zhǔn)確性，通過(guò)多次測(cè)量取平均值。表2為試驗(yàn)環(huán)境，使用型號(hào)為B7104Q3H7GKM00T5的LAFERT電機(jī)，Intel i5處理器計(jì)算機(jī)，內(nèi)存8 G，win10系統(tǒng)，控制平臺(tái)由運(yùn)動(dòng)控制軟件TwinCAT構(gòu)建。

3.2 不同控制方法對(duì)比分析

將文中控制方法與傳統(tǒng)的PID控制方法以及傳統(tǒng)的PID+前饋控制方法進(jìn)行比較。通過(guò)從站的伺服驅(qū)動(dòng)器獲得分揀機(jī)器人電機(jī)編碼器的關(guān)節(jié)位置，比較分揀機(jī)器人的4個(gè)關(guān)節(jié)的指令位置與實(shí)際位置，并對(duì)差值進(jìn)行數(shù)據(jù)處理。圖5為分揀機(jī)器人采用不同控制方法的4個(gè)驅(qū)動(dòng)關(guān)節(jié)的位置跟蹤誤差，不同控制方法下各軸位置跟隨誤差的絕對(duì)值最大值和平均值見(jiàn)表3。

圖4 力矩前饋控制模型

表2 試驗(yàn)參數(shù)

由圖5和表3可知，相比于傳統(tǒng)的PID控制方法，加入前饋控制方法后可將最大和平均關(guān)節(jié)位置跟蹤誤差降低超過(guò)35%。加入自整定和前饋控制的方法最大和平均位置跟蹤誤差降低超過(guò)65%，極大地提高了各關(guān)節(jié)的跟蹤能力，從而實(shí)現(xiàn)了分揀機(jī)器人的高精度控制。圖6為3種控制方法下分揀機(jī)器人末端執(zhí)行器在工作空間中的軌跡誤差比較，由關(guān)節(jié)位置正向運(yùn)動(dòng)學(xué)正解得到。

圖5 不同控制方法的空間跟隨誤差對(duì)比

表3 不同控制方法位置跟隨誤差最大值和平均值

圖6 不同控制方法軌跡誤差對(duì)比

由圖6可知，傳統(tǒng)PID控制方法最大軌跡誤差為0.192 mm，使用傳統(tǒng)的PID+前饋控制方法的最大軌跡誤差為0.143 mm，降低了25.52%；使用遺傳算法優(yōu)化的PID+前饋控制方法的最大軌跡誤差為0.094 mm，降低了51.04%。

綜上，通過(guò)前饋控制可以提高系統(tǒng)控制精度，但無(wú)法解決系統(tǒng)模型誤差，因此通過(guò)自適應(yīng)整定可以進(jìn)一步提高控制精度，而遺傳規(guī)則正好滿足此要求。通過(guò)動(dòng)態(tài)前饋補(bǔ)償和遺傳優(yōu)化的PID反饋調(diào)節(jié)相結(jié)合可以有效提高系統(tǒng)的控制精度。為了進(jìn)一步驗(yàn)證文中控制方法的有效性，對(duì)控制關(guān)節(jié)的實(shí)測(cè)轉(zhuǎn)矩和理論轉(zhuǎn)矩進(jìn)行進(jìn)一步比較，結(jié)果如圖7所示。

由圖7可知，實(shí)測(cè)力矩曲線與理論值曲線非常接近，且峰值與趨勢(shì)基本相同，證實(shí)了文中控制方法的有效性。對(duì)誤差產(chǎn)生的原因進(jìn)行分析，可能是由模型參數(shù)誤差、環(huán)境噪音等引起的。

圖7 關(guān)節(jié)實(shí)測(cè)力矩與理論值對(duì)比

4 結(jié)論

提出了一種將傳統(tǒng)的閉環(huán)運(yùn)動(dòng)學(xué)控制和力矩前饋控制相結(jié)合的高速并聯(lián)食品分揀機(jī)器人控制方法。在原有遺傳算法整定PID控制參數(shù)的基礎(chǔ)上，通過(guò)力矩前饋控制方法進(jìn)行動(dòng)態(tài)控制。結(jié)果表明，與傳統(tǒng)的PID控制相比，關(guān)節(jié)位置跟蹤最大和平均誤差降低超過(guò)65%，最大軌跡誤差降低超過(guò)50%，極大地提高了高速并聯(lián)食品分揀機(jī)器人各關(guān)節(jié)的跟蹤能力，實(shí)現(xiàn)了各關(guān)節(jié)的高精度控制。高速并聯(lián)食品分揀機(jī)器人的控制方法仍處于試驗(yàn)階段，文中僅對(duì)跟蹤精度和關(guān)節(jié)轉(zhuǎn)矩進(jìn)行比較和分析，未對(duì)加速度變化和負(fù)載變化進(jìn)行研究，后續(xù)將注重提高控制系統(tǒng)性能，完善高速并聯(lián)食品分揀機(jī)器人系統(tǒng)。