碼垛機(jī)器人的軌跡規(guī)劃與仿真分析*

葉辰雷，劉曉平，贠 超

(1.北京郵電大學(xué)自動化學(xué)院，北京 100876;2.北京航空航天大學(xué)機(jī)器人研究所，北京 100191)

碼垛機(jī)器人的軌跡規(guī)劃與仿真分析*

葉辰雷1，劉曉平1，贠 超2

(1.北京郵電大學(xué)自動化學(xué)院，北京 100876;2.北京航空航天大學(xué)機(jī)器人研究所，北京 100191)

針對碼垛機(jī)器人在高速運(yùn)行過程中產(chǎn)生的沖擊問題，以優(yōu)化運(yùn)行節(jié)拍提高效率為目的對碼垛機(jī)器人的運(yùn)動軌跡進(jìn)行規(guī)劃。利用笛卡爾空間圓弧插補(bǔ)方法對運(yùn)行軌跡進(jìn)行優(yōu)化，并在此基礎(chǔ)上利用三次曲線插補(bǔ)對r關(guān)節(jié)運(yùn)動軌跡進(jìn)行了再次修正，基于Matlab建立模型對規(guī)劃結(jié)果進(jìn)行了仿真。結(jié)果表明通過軌跡曲線規(guī)劃方法使得碼垛機(jī)器人在運(yùn)行過程當(dāng)中滿足了減小運(yùn)行沖擊和提高運(yùn)行效率的要求。

碼垛機(jī)器人;軌跡規(guī)劃;圓弧插補(bǔ);仿真曲線

1 引言

碼垛機(jī)器人技術(shù)作為自動化物流技術(shù)的重要環(huán)節(jié)，對于提升生產(chǎn)效率，降低市場成本，推進(jìn)生產(chǎn)方式現(xiàn)代化有著重要意義。碼垛機(jī)器人對于響應(yīng)時間和沖擊力的控制只有在相對應(yīng)的運(yùn)動軌跡之下，才能同時保證運(yùn)動速度的高效和機(jī)械本體所受到?jīng)_擊最小。

通過進(jìn)行有效地軌跡規(guī)劃，對于保障運(yùn)行過程中的運(yùn)動平穩(wěn)和時間要求［1］，提高碼垛機(jī)器人的運(yùn)行效率和穩(wěn)定性起著關(guān)鍵性的作用，筆者采用笛卡爾空間圓弧圓的滑過渡方法來保證運(yùn)動軌跡路徑的規(guī)劃，圓弧軌跡在運(yùn)動過程中避免了沖擊，在空間軌跡規(guī)劃之后又對規(guī)劃結(jié)果利用三次曲線插補(bǔ)的關(guān)節(jié)軌跡規(guī)劃做了二次修正，針對課題要求的運(yùn)動軌跡結(jié)合實(shí)際運(yùn)動規(guī)范進(jìn)行了理論推導(dǎo)，根據(jù)相關(guān)理論推倒的結(jié)果聯(lián)合Matlab編程給出了數(shù)學(xué)仿真數(shù)據(jù)結(jié)論，驗(yàn)證了規(guī)劃方案的可行性。

2 碼垛機(jī)器人的結(jié)構(gòu)和運(yùn)動軌跡

2.1 碼垛機(jī)器人的設(shè)計結(jié)構(gòu)

碼垛機(jī)器人的結(jié)構(gòu)其具體結(jié)構(gòu)如圖1所示，該機(jī)器人的手臂，固定在腰部上，在該部分內(nèi)小臂通過前大臂、后大臂與的搬運(yùn)和碼垛作業(yè)，且機(jī)械系統(tǒng)主要有4個關(guān)節(jié)部分組成，能實(shí)現(xiàn)四種運(yùn)動:腰部旋轉(zhuǎn)，大臂上下運(yùn)動，小臂前后運(yùn)動和手腕回轉(zhuǎn)運(yùn)動四種運(yùn)動，全部由交流伺服電機(jī)驅(qū)動。

圖1 機(jī)器人機(jī)構(gòu)簡圖

2.2 碼垛機(jī)器人的運(yùn)動

對于自由度大于3的常規(guī)工業(yè)機(jī)器人而言，其運(yùn)動參數(shù)的設(shè)計及計算不唯一［2］。本文涉及的碼垛機(jī)器人所采用的是基于“取－放”這一工作模式的要求進(jìn)行的設(shè)計，實(shí)際工作情況下采取的是“4－3－4”的工作模式。即剛開始時位于初始位置O，抓取時抓手位于流水線上方位置等待抓取這一位置為抓取準(zhǔn)備點(diǎn)P2，當(dāng)有貨物來到時從P2點(diǎn)垂直向下運(yùn)動至抓取點(diǎn)P1，抓取貨物，之后返回P2位置再進(jìn)行下一步的動作，垂直提升一段距離以確保做搬運(yùn)運(yùn)動時抓手不與流水線運(yùn)輸帶發(fā)生碰撞;之后由抓取準(zhǔn)備點(diǎn)P2向放置準(zhǔn)備點(diǎn)P3做水平運(yùn)動;再由P3點(diǎn)向預(yù)先計算的放置點(diǎn)P4做垂直運(yùn)動到達(dá)之后抓手打開，貨物按一定姿態(tài)放置于垛型之中。其中P3、P4是根據(jù)垛型不同位置，每次放時重新計算生成的點(diǎn)，如圖2所示。

圖2 碼垛機(jī)器人的運(yùn)動示意圖

2.3 碼垛機(jī)器人實(shí)際運(yùn)動軌跡

碼垛機(jī)器人堆垛的過程，在堆垛過程當(dāng)中，機(jī)器人運(yùn)行采用點(diǎn)到點(diǎn)運(yùn)行，即采用PTP的方式，即只需要確定各段運(yùn)動軌跡的端點(diǎn)，而端點(diǎn)之間運(yùn)動軌跡由各軸電機(jī)獨(dú)立控制，因此實(shí)際運(yùn)動曲線在空間當(dāng)中并不是如示意圖中的折線，而是隨機(jī)的曲線，同時由于在必須到達(dá)準(zhǔn)備點(diǎn)這一要求，在準(zhǔn)備點(diǎn)處極有可能形成銳利的夾角，在實(shí)際空間運(yùn)動中對于機(jī)械結(jié)構(gòu)和電機(jī)都會構(gòu)成較大的力和扭矩的沖擊，會造成機(jī)械本體的震動同時又影響到運(yùn)行速度和節(jié)拍。實(shí)際運(yùn)動曲線如圖3所示。

3 軌跡規(guī)劃的理論方法及規(guī)劃方案的確定

實(shí)際運(yùn)動過程當(dāng)中碼垛機(jī)器人運(yùn)動的目標(biāo)點(diǎn)是依據(jù)兩方面構(gòu)成的:首先是控制試教點(diǎn)位置確定抓取點(diǎn)以及放置點(diǎn);其次是通過計算得到抓取準(zhǔn)備點(diǎn)的位置和第一放置準(zhǔn)備點(diǎn)，以及通過垛型規(guī)劃計算確定的其余放置點(diǎn)。因此在每次行程工作中碼垛機(jī)器人的運(yùn)動都是由前文中提供P1、P2、P3、P4四個點(diǎn)位置構(gòu)成的，在軌跡設(shè)計當(dāng)中有以下要求:①軌跡必須包含4個位置點(diǎn);②避免銳利的轉(zhuǎn)角和折線，在空間內(nèi)行程應(yīng)當(dāng)是一條圓滑曲線;③通過插補(bǔ)的方式確保所有點(diǎn)到點(diǎn)的距離相等。

綜合上述三點(diǎn)，通過空間4點(diǎn)位置插補(bǔ)兩段圓弧，根據(jù)設(shè)定的運(yùn)動軌跡方程計算符合要求的插補(bǔ)點(diǎn)坐標(biāo)，通過坐標(biāo)轉(zhuǎn)換將空間軌跡轉(zhuǎn)化為關(guān)節(jié)的運(yùn)動軌跡，將轉(zhuǎn)換的圓柱坐標(biāo)發(fā)送至PMAC卡，執(zhí)行運(yùn)動程序?qū)崿F(xiàn)軌跡的規(guī)劃。由圖4可知，4個點(diǎn)構(gòu)成的三條直線位于同一平面內(nèi)，由于P2與P3兩點(diǎn)作為準(zhǔn)備點(diǎn)，必須通過，因此在軌跡規(guī)劃中增加兩點(diǎn)Pa與Pb，其坐標(biāo)位于準(zhǔn)備點(diǎn)z軸坐標(biāo)上方。對直線與直線的連接處做圓弧插補(bǔ)要求既要圓弧位于同一平面內(nèi)，又要求圓弧與直線的切點(diǎn)在z軸方向上不小于準(zhǔn)備點(diǎn)。

圖4 軌跡規(guī)劃曲線

4 軌跡插補(bǔ)的理論推導(dǎo)及計算

4.1 空間圓弧軌跡的插補(bǔ)

4.1.1 圓弧位置點(diǎn)的計算

已知空間不共線三點(diǎn)坐標(biāo)，求由三點(diǎn)組成的兩條線斷的插補(bǔ)圓弧情況如圖5所示，插補(bǔ)圓弧要求已知空間三點(diǎn)P1(χ1，y1，z1)，P2(χ2，y2，z2)，P3(χ3，y3，z3)三點(diǎn)分別構(gòu)成的兩條直線，假設(shè)圓心點(diǎn)坐標(biāo)為Po(a，b，c)，根據(jù)空間能不共線的三個點(diǎn)可以確定一個平面可確定方程為:

同時假設(shè)圓弧的起始點(diǎn)Pa(χa，ya，za)和終止點(diǎn)Pb(χb，yb，zb)。根據(jù)過度圓弧的圓心到切點(diǎn)的距離等于半徑r，可聯(lián)立方程組:

又可知，過度圓弧的圓心與切點(diǎn)的連線垂直于相切直線，可聯(lián)立方程組:

通過上述5個方程可以求解，由于需要求解的變量有Po(a，b，c)、Pa(χa，ya，za)和Pb(χb，yb，zb)，共有9個未知量，因此還需要再引入兩個比例系數(shù)來表示切點(diǎn)坐標(biāo)。已知 Pa與 Pb分別位于線段 P1P2和P2P3上，引入兩個比例系數(shù)k1和k2，這樣就可以將Pa(χa，ya，za)和Pb(χb，yb，zb)的六個未知量用k1和k2及P1(χ1，y1，z1)，P2(χ2，y2，z2)，P3(χ3，y3，z3)來表示。并且從圖中可知，切點(diǎn)始終位于線段兩點(diǎn)之間，因此要求將0＜k1＜1和0＜k2＜1作為篩選條件，最終得到圓心坐標(biāo)和兩個切點(diǎn)的坐標(biāo)值。

圖5 圓弧位置計算示意

4.1.2 圓弧插補(bǔ)的計算推導(dǎo)

通過已知起始點(diǎn)，終止點(diǎn)和圓心點(diǎn)的坐標(biāo)，同時根據(jù)已設(shè)定的半徑插補(bǔ)圓弧上的距離求取插補(bǔ)點(diǎn)坐標(biāo)。

(1)確定所要插補(bǔ)圓弧點(diǎn)的數(shù)量，以給定各個點(diǎn)之間距離l，通過計算個點(diǎn)之間圓弧段距離判斷圓弧所需要插補(bǔ)點(diǎn)數(shù)量，首先由圓弧起始點(diǎn)點(diǎn)原點(diǎn)終止點(diǎn)所構(gòu)成的弧如公式S=θr所得，通過均分計算計算所需插補(bǔ)點(diǎn)N=S/l，為確保插補(bǔ)足夠數(shù)量向上取整數(shù)。

(2)通過向量求出圓弧插補(bǔ)方向通過已知起始點(diǎn)A終止點(diǎn)C及圓弧圓心B，可以利用≤90°來表示圓弧所在平面的法向量n，由于所得插補(bǔ)圓弧始終插補(bǔ)于兩條直線之間，所以可以得知∠ABC≤90°，由于法向量方向由右手定則確定，具體方法是右手拇指與其余四指垂直，握拳時四指運(yùn)動的方向表示從第一向量到第二向量，拇指所指方向就是法向量的方向。確定插補(bǔ)總是以法向量方向沿順時針方向，因此只需要確定圓弧起始點(diǎn)與終止點(diǎn)位置利用插補(bǔ)方程計算插補(bǔ)點(diǎn)坐標(biāo)。如圖6所示，在A為起始點(diǎn)C為終止點(diǎn)的情況下，法向量的順時針沿A指向C;反之法沿C指向A，因此在不同的環(huán)境下都可以表示出圓弧所要插補(bǔ)圓弧的移動方向。

(3)根據(jù)圓弧插補(bǔ)遞推方程插補(bǔ)圓弧并修正誤差通過圓弧起始點(diǎn)、圓心和終止點(diǎn)確定圓弧向量方向，在做切向插補(bǔ)遞推時，可以看出在做切向運(yùn)動的，如圖7所示。

其中所得(mi，ni，li)為切向進(jìn)給方向，根據(jù)切向距離l可以求出切向運(yùn)動的位置坐標(biāo)Pi+1'如下:

圖6 圓弧方向的確定

圖7 圓弧遞推插補(bǔ)示意

可以通過公式計算出每個插補(bǔ)點(diǎn)的空間位置坐標(biāo)，此種插補(bǔ)方法插補(bǔ)點(diǎn)總在圓弧上，不存在累積誤差，但由于存在插補(bǔ)點(diǎn)不能再到達(dá)終止點(diǎn)時確定插補(bǔ)到位，存在最終插補(bǔ)點(diǎn)偏離終止點(diǎn)的問題，因此對于終止點(diǎn)的位置進(jìn)行修正，用計算所得的終止點(diǎn)坐標(biāo)替換插補(bǔ)遞推點(diǎn)的坐標(biāo)，確保插補(bǔ)點(diǎn)的位置在所要求的范圍之內(nèi)。

4.2 空間直線軌跡的插補(bǔ)

首先要確定直線坐標(biāo)的起始點(diǎn)A和終止點(diǎn)B根據(jù)要求的點(diǎn)和點(diǎn)之間的距離要求l來先確定插補(bǔ)點(diǎn)的數(shù)量，通過求取兩點(diǎn)間的距離可以得到的直線距離;其次對于距離求取點(diǎn)的個數(shù)并且取上限，同時利用等比例插補(bǔ)的原理分別求出插補(bǔ)點(diǎn)的空間坐標(biāo)，其計算公式如下:

其中:i表示各個點(diǎn)n表示所插補(bǔ)點(diǎn)的總數(shù)量。最后利用替換最后一點(diǎn)為給定終止點(diǎn)確保插補(bǔ)點(diǎn)的位置信息位于規(guī)劃軌跡范圍內(nèi)。

5 軌跡的仿真分析及二次修正

5.1 規(guī)劃軌跡的仿真和分析

根據(jù)上述方法可以求出規(guī)劃軌跡插補(bǔ)點(diǎn)的空間坐標(biāo)，利用這些點(diǎn)使用Matlab可以繪制出仿真空間軌跡如圖8所示圖中曲線所取插補(bǔ)圓弧半徑r=50 cm，插補(bǔ)點(diǎn)間距l(xiāng)=10 cm。圖中黑色的軌跡為原軌跡，圓點(diǎn)為規(guī)劃軌跡。可以證明在貨物提升之后到貨物搬運(yùn)過程的曲線由之前的尖銳夾角過渡成為平滑的圓弧，避免了由于尖銳過度造成的問題。

圖8 軌跡規(guī)劃空間曲線

對于碼垛機(jī)器人運(yùn)動，其關(guān)節(jié)運(yùn)動直接關(guān)系到電機(jī)選型及運(yùn)動速度極限問題，通過坐標(biāo)系的轉(zhuǎn)化和運(yùn)動學(xué)反解，我們可以通過笛卡爾坐標(biāo)系運(yùn)動軌跡點(diǎn)得到控制位置的三個電機(jī)的關(guān)節(jié)運(yùn)動曲線運(yùn)動情況，如圖9為軌跡仿真曲線。

碼垛機(jī)器人作為一種點(diǎn)對點(diǎn)的搬運(yùn)機(jī)器設(shè)備，在實(shí)際運(yùn)動中被關(guān)心的更多是運(yùn)動速度和運(yùn)動起始與終止位置。在規(guī)劃運(yùn)動曲線圖9中可以看出在實(shí)際關(guān)節(jié)運(yùn)動過程中在r軸方向上的運(yùn)動相較于未進(jìn)行規(guī)劃的運(yùn)動在運(yùn)動曲線上更加平滑，但同時也可見其運(yùn)動速度曲線絕對幅值變大，造成這一變化的原因是由于插補(bǔ)軌跡曲線是利用4個點(diǎn)坐標(biāo)來確定的曲線坐標(biāo)，而曲線位于四個點(diǎn)所構(gòu)成的平面內(nèi)，而碼垛機(jī)器人的運(yùn)動坐標(biāo)系是柱坐標(biāo)系，變量θ對應(yīng)柱坐標(biāo)系中的偏轉(zhuǎn)角，關(guān)節(jié)變量χ對應(yīng)柱坐標(biāo)系中的距離在軸距離r，關(guān)節(jié)變量z對應(yīng)柱坐標(biāo)系當(dāng)中z。因此在實(shí)際運(yùn)動情況下，我們分別考慮三個關(guān)節(jié)運(yùn)動曲線的實(shí)際情況。由規(guī)劃曲線我們可以看出:根據(jù)現(xiàn)有軌跡運(yùn)動，r值得變化存在一個先變小后變大的過程，這個過程實(shí)際上對于電機(jī)運(yùn)轉(zhuǎn)是多余的，而會引起加減速過程從而減緩運(yùn)動速度。

圖9 軌跡規(guī)劃關(guān)節(jié)曲線

從上述原因所導(dǎo)致的結(jié)果——在實(shí)際運(yùn)動中所做的運(yùn)動路程更長，速度變化也更大。這點(diǎn)不符合軌跡規(guī)劃要求運(yùn)動結(jié)果時間優(yōu)化的要求，而更大的運(yùn)動變化同時也會造成更明顯的震動和沖擊，考慮到上述問題，有必要對 r關(guān)節(jié)的運(yùn)動曲線進(jìn)行重新規(guī)劃［5－6］。

5.2 利用三次曲線插補(bǔ)的二次修正計算

對于上述問題本文利用三次曲線插補(bǔ)的方法進(jìn)行處理。三次曲線具有二階可導(dǎo)性，其表達(dá)方式保證了其一階求導(dǎo)結(jié)果即速度曲線平滑和二階求導(dǎo)結(jié)果即加速度曲線的連續(xù)性，在原有插補(bǔ)點(diǎn)基礎(chǔ)上可以保證所得結(jié)果符合路程運(yùn)動和速度曲線的要求。

(1)三次曲線方程 已知給定數(shù)量的插補(bǔ)點(diǎn)n，對于各個位姿點(diǎn)的變化量為Qi(i=1，2，……，n)，各連續(xù)點(diǎn)的運(yùn)動時間為ti(i=1，2，……，n)其間隔時間為T。由上述已知量可以推導(dǎo)出插補(bǔ)曲線的方程為:

在修正曲線的計算當(dāng)中，規(guī)劃的曲線是基于之前所得到的插補(bǔ)點(diǎn)進(jìn)行的，即修正曲線插補(bǔ)點(diǎn)的數(shù)量與之前插補(bǔ)點(diǎn)的數(shù)量為一一對應(yīng)的關(guān)系，因此在計算過程中，公式ti=n×T，其中T為確定插補(bǔ)點(diǎn)時間間隔常量。當(dāng)確定了插補(bǔ)點(diǎn)的坐標(biāo)位置要求、速度要求和插補(bǔ)點(diǎn)要求，可得到所求三次曲線方程［7－8］。

(2)求解三次曲線方程系數(shù) 根據(jù)本文中所涉及的關(guān)節(jié)空間軌跡修正方法，對于方程插補(bǔ)確定起始點(diǎn)位置坐標(biāo)Q1(t1)和速度Q1'(t1)以及終止點(diǎn)的位置坐標(biāo)Qn(tn)和速度Qn'(tn)可得到方程組:

規(guī)劃中勝定速度在起始點(diǎn)和終止點(diǎn)皆為0，因此由方程式(12)可以求出所需要的A、B、C、D四個未知量以確定三次曲線的方程，且通過已知的三次曲線修正之前的r位置坐標(biāo)各個點(diǎn)，從而達(dá)到對軌跡規(guī)劃曲線的二次修正。

5.3 修正后的仿真及分析

根據(jù)上文所提供的算法結(jié)合仿真數(shù)據(jù)對插補(bǔ)軌跡曲線做修正將位于規(guī)劃平面內(nèi)的插補(bǔ)曲線做出二次軌跡修正規(guī)劃。最后得到軌跡空間運(yùn)動曲線仿真如圖10所示。

圖10 軌跡規(guī)劃空間及關(guān)節(jié)運(yùn)動曲線

通過關(guān)節(jié)運(yùn)動軌跡圖10可以看出，r關(guān)節(jié)軌跡運(yùn)動滿足要求，不再出現(xiàn)富余運(yùn)動。速度曲線圖也顯示速度變化更加平穩(wěn)，幅值變化不大。

6 結(jié)論

通過對空間機(jī)器人運(yùn)動軌跡規(guī)劃方法結(jié)合碼垛機(jī)器人的自身特點(diǎn)和實(shí)際運(yùn)動需求進(jìn)行設(shè)計的軌跡規(guī)劃方案，利用直線插補(bǔ)和圓弧插補(bǔ)計算結(jié)合數(shù)學(xué)算法和規(guī)劃要求推導(dǎo)計算公式;從而得到了插補(bǔ)點(diǎn)的空間軌跡位置集合。通過利用Matlab數(shù)學(xué)計算工具對計算方案進(jìn)行了模擬仿真，通過比較仿真結(jié)果針對相關(guān)規(guī)劃曲線做了二次修正，驗(yàn)證了修正結(jié)論的可行性，證明軌跡規(guī)劃的相關(guān)要求。對于提高碼垛機(jī)器人的運(yùn)動效率減小運(yùn)行中的沖擊振動問題起到了重要作用。

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

［2］ 馬香峰.機(jī)器人機(jī)構(gòu)學(xué)［M］.北京:機(jī)械工業(yè)出版社，1991.

［3］ 原 魁.工業(yè)機(jī)器人發(fā)展現(xiàn)狀與趨勢［J］.工廠自動化，2007，1(1):34－38.

［4］ 余達(dá)太，馬香峰.工業(yè)機(jī)器人應(yīng)用工程［M］.北京:冶金工業(yè)出版社，1999.

［5］ 胡洪國，高建華.碼垛技術(shù)綜述［J］.組合機(jī)床與自動化加工技術(shù)，2000(6):7－9.

［6］ Zlajpah L，Nemec B.Implementation of time－optimal path－tracking control on palletizing robots［C］//Industrial Electronics，1999.ISIE'99.Proceedings of the IEEE International Symposium on.IEEE，1999(2):861－866.

［7］ Lin C，Chang P，Luh J.Formulation and optimization of cubic polynomial joint trajectories for industrial robots［J］.Automatic Control，IEEE Transactions on，1983，28(12):1066－1074.

［8］ 陳 航，殷國富，趙 偉，等.工業(yè)機(jī)器人模塊化設(shè)計研究［J］.機(jī)械，2009(3): 57－59.

Trajectory Planning and Simulation for Palletizing Robot

YE Chen－lei1，LIU Xiao－ping1，YUN Chao2
(1.School of Automation，Beijing University of Posts and Telecommunications Institute of Automation，Beijing 100876，China; 2 Robotics Institute，Beihang University，Beijing 100191，China)

Aiming at the impact problems at the process of high－speed operation of the palletzing robot，trajectory of palletizing robot is planned in order to optimize operational tempo and improve the efficiency.Arc interpolation methods are used to optimize running track in a Cartesian coordinate system.On this basis，r－joint trajectory is corrected secondary using cubic spline interpolation，the planning conclusion is simulated based on Matlab model.The results show that palletizing robot operation meets the requirements of reducing impact and improves the operation efficiency by this trajectory planning method.

palletizing robot;trajectory planning;circular arc interpolation;simulation curve

TP242

A

1007－4414(2013)05－0026－05

2013－09－03

葉辰雷(1988－)，男，云南昆明人，碩士，主要從事測控技術(shù)與結(jié)構(gòu)動態(tài)設(shè)計方面的研究。