基于Delta 并聯(lián)機(jī)器人的傳送帶動(dòng)態(tài)跟蹤?

鄧明星，劉冠峰，張國(guó)英

（廣東工業(yè)大學(xué) 機(jī)電工程學(xué)院，廣東 廣州 510006）

0 引言

工業(yè)機(jī)器人技術(shù)的發(fā)展和成熟，正孕育著新一輪的產(chǎn)業(yè)革命。Delta機(jī)器人是一種高速、輕載的并聯(lián)機(jī)器人，多應(yīng)用在電子、食品、生物制藥等包裝行業(yè)。目前針對(duì)機(jī)器人對(duì)傳送帶的動(dòng)態(tài)跟蹤研究比較少，本文提出了一種基于Delta并聯(lián)機(jī)器人的新型跟蹤算法。

1 Delta機(jī)器人硬件系統(tǒng)

如圖1所示，本文的機(jī)器人是Delta并聯(lián)機(jī)器人，機(jī)器人下方有多條傳送帶，傳送帶的位置在機(jī)器人的工作空間內(nèi)；除此之外，還有一個(gè)視覺(jué)檢測(cè)系統(tǒng)，該系統(tǒng)負(fù)責(zé)定位傳送帶上物體的位姿。

圖1 Delta并聯(lián)機(jī)器人傳送帶跟蹤系統(tǒng)

如圖2所示，把傳送帶定義為可移動(dòng)的坐標(biāo)系C，該坐標(biāo)系x方向?yàn)閭魉蛶б苿?dòng)方向，沿x方向，順流而下為Downstream，逆流而上為Upstream。在圖1中，把視覺(jué)系統(tǒng)定義成坐標(biāo)系V，機(jī)器人坐標(biāo)系為R。通過(guò)視覺(jué)標(biāo)定和傳送帶標(biāo)定，可分別確定V到R和C到R的轉(zhuǎn)換矩陣。故而，可確定C和R的關(guān)系。

視覺(jué)系統(tǒng)定位的物體位置矢量為VP，在R中的位置為［1］：

最終得到物體的實(shí)際位置為：

圖2 傳送帶的定義和區(qū)域

2 PID算法引進(jìn)

當(dāng)系統(tǒng)要求較高時(shí)，常常采用有源校正環(huán)節(jié)，而PID調(diào)節(jié)器已經(jīng)形成了典型結(jié)果，其參數(shù)整定簡(jiǎn)單，在許多工業(yè)過(guò)程控制中獲得了較為滿意的效果。所謂的PID控制規(guī)律，就是一種對(duì)偏差ε（t）進(jìn)行比例、積分和微分變化的控制規(guī)律［2］：

其中：m（t）為t時(shí)刻系統(tǒng)的調(diào)整量；Kp為系統(tǒng)的比例系數(shù)；Ti和Td分別為系統(tǒng)的積分時(shí)間常數(shù)和微分時(shí)間常數(shù)。

值得注意的是：①比例系數(shù)Kp直接決定控制作用的強(qiáng)弱，加大Kp可以減小系統(tǒng)的穩(wěn)態(tài)誤差；②在比例環(huán)節(jié)的基礎(chǔ)上，由積分時(shí)間常數(shù)Ti控制的積分環(huán)節(jié)可以消除系統(tǒng)的穩(wěn)態(tài)誤差，但控制過(guò)強(qiáng)會(huì)使系統(tǒng)的超調(diào)量增大，導(dǎo)致系統(tǒng)的穩(wěn)定性變差；③微分控制能夠預(yù)測(cè)偏差，微分時(shí)間常數(shù)Td越大，響應(yīng)速度越快，噪聲信號(hào)也越大。

3 基于PID的跟蹤算法

在本文中，假如物體在傳送帶的上游，當(dāng)其進(jìn)入視覺(jué)系統(tǒng)的視場(chǎng)之后，定位得到物體的瞬時(shí)位置為VP（x0，y0，z0），通過(guò)式（1）～式（4）轉(zhuǎn)換，可得到拍照時(shí)刻物體在機(jī)器人坐標(biāo)系下的位置RP（x1，y1，z1）［3－4］，任意時(shí)刻機(jī)器人末端的位置為RPr0（xr0，yr0，zr0）；而RP＋ve（t）－ve（0）即為機(jī)器人任意時(shí)刻末端的目標(biāo)位置，因此：

其中：ve（t），ve（0）分別為t時(shí)刻和拍照時(shí)傳送帶編碼器的讀數(shù)［5］。

設(shè)εx（t）、εy（t）、εz（t）分別為ε（t）在機(jī)器人坐標(biāo)系中x，y，z軸的分量：

其中：θ，γ分別為傳送帶與x軸和xoy平面的夾角。下一個(gè)周期時(shí)，機(jī)器人末端將要到的位置為Pn：

需采用離散的方法對(duì)時(shí)間積分，每隔一個(gè)采樣周期Δt，更新一下Pn，如采樣周期足夠短，式（8）即被看作連續(xù)的，因此機(jī)器人在t時(shí)刻的調(diào)整量m（t）可以寫成：

4 測(cè)試基本的PID算法

用式（8）來(lái)做跟蹤測(cè)試，結(jié)果如圖3所示。在跟蹤過(guò)程中，機(jī)器人與目標(biāo)位置的偏差逐漸縮小，最后收斂。剛開(kāi)始時(shí)機(jī)器人會(huì)出現(xiàn)劇烈振蕩，原因是當(dāng)前位置與目標(biāo)位置的偏差較大，為立即減小偏差，機(jī)器人增大了調(diào)整幅度，導(dǎo)致其出現(xiàn)抖動(dòng)現(xiàn)象，并且會(huì)持續(xù)一段時(shí)間［6］。

圖3 Td調(diào)整前的跟蹤偏差圖

為了抑制系統(tǒng)的振蕩現(xiàn)象，可考慮逐漸減小Td的值［7］。調(diào)整后的結(jié)果如圖4所示。

經(jīng)Td調(diào)整后，系統(tǒng)的穩(wěn)定性和動(dòng)態(tài)響應(yīng)得到了明顯的改善。但傳送帶移動(dòng)速度快慢不均［8］，導(dǎo)致機(jī)器人自調(diào)整不夠及時(shí)和平滑，從而出現(xiàn)明顯的滯后和抖動(dòng)現(xiàn)象。

5 改進(jìn)的PID跟蹤算法

由于基本的PID跟蹤算法在速度突變時(shí)機(jī)器人的反應(yīng)有一定的滯后，因此，提出了改進(jìn)的跟蹤算法，使機(jī)器人能夠時(shí)刻做好應(yīng)對(duì)“速度突變”的準(zhǔn)備［9］。

圖4 Td調(diào)整后的跟蹤偏差圖

其中：Ta為加速度調(diào)整系數(shù)；Td1為速度平方的調(diào)整系數(shù)。為確保跟蹤更準(zhǔn)確，可使用連續(xù)兩個(gè)周期的平均值來(lái)求得ε（t）和預(yù)測(cè)速度。引進(jìn)以下公式：

測(cè)試式（10）、式（11），得到的結(jié)果如圖5、圖6所示。

圖5 基于速度二次曲線的跟蹤偏差圖

圖5中，系統(tǒng)表現(xiàn)良好，振動(dòng)較少，誤差大致被控制在±4mm內(nèi)，穩(wěn)定性較高。

圖6中，機(jī)器人的跟蹤精度得到大幅度提升，誤差控制在±1mm內(nèi)，可以看出，由于加速度的引進(jìn)，有效地抑制了系統(tǒng)的滯后。

6 結(jié)束語(yǔ)

由實(shí)驗(yàn)結(jié)果可知，改進(jìn)的PID算法基本能實(shí)現(xiàn)機(jī)器人對(duì)傳送帶的跟蹤。機(jī)器人系統(tǒng)的穩(wěn)定性、快速響應(yīng)性表現(xiàn)良好，將會(huì)對(duì)Delta機(jī)器人跟蹤算法的發(fā)展產(chǎn)生一定的影響。

圖6 基于加速度的跟蹤偏差圖

［1］趙杰，朱延河，蔡鶴皋，等.Delta型并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)正解集合方法［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2003，35（1）：25-27.

［2］楊叔子，楊克沖.機(jī)械工程控制［M］.武漢：華中科技大學(xué)出版社，2005.

［3］蔡自興.機(jī)器人學(xué)［M］.北京：清華大學(xué)出版社，2009.

［4］于東興.高速機(jī)器人飲料包裝生產(chǎn)線技術(shù)及其關(guān)鍵裝備研究［D］.天津：天津大學(xué)，2011：35-37.

［5］Wilhelm W E.Conveyor tracking［M］//International Encyclopedia of Robotics：Applications and Automation.New York：Wiley，1988：283-298.

［6］Shin K G，Mckay N D.A dynamic programming approach to trajectory planning of robotic manipulators［J］.IEEE Trans Automatic Contr，1986，31（9）：491-500.

［7］Taylor R H.Planning and execution of straight line manipulator trajectories［J］.IBM J Res Develop，1979，23（4）：424-436.

［8］Park T H，Lee B H.An approach to robot motion analysis and planning for conveyor tracking［J］.IEEE Transaction on Systems，Man and Cybernetics，1992，22（2）：378-384.

［9］Ik Sang Shin，Sang-Hyun Nam，Hyun Geun Yu.Conveyor visual tracking using robot vision［C］//Florida Conference on Recent Advances in Robotics.Florida：［s.n.］，2006：1-5.