一種碼垛機(jī)器人弧形運(yùn)動(dòng)軌跡算法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及碼垛機(jī)器人，尤其是一種碼垛機(jī)器人弧形運(yùn)動(dòng)軌跡算法。
【背景技術(shù)】
[0002]碼垛機(jī)器人主要應(yīng)用于大負(fù)載，高速環(huán)境替代人工作業(yè)的場(chǎng)合，其最突出的問題就是要節(jié)拍快，節(jié)能，柔性等。如圖1所示，傳統(tǒng)的碼垛動(dòng)作基本上都是基于“門”字形機(jī)構(gòu)的動(dòng)作軌跡。動(dòng)作過程分為三步，第一步直線向下抓取，然后拉起來平移，到目標(biāo)位置時(shí)垂直放下，然后按照相反的動(dòng)作返回，繼續(xù)下一個(gè)節(jié)拍。由于每次都是經(jīng)過直線運(yùn)動(dòng)插補(bǔ)時(shí)機(jī)器人運(yùn)動(dòng)比較機(jī)械化，不夠柔性，另外行走路徑是直角中間有兩個(gè)拐點(diǎn)，半個(gè)周期內(nèi)要有三次加減速，所以此方式動(dòng)作耗時(shí)速度慢，不夠柔性。

【發(fā)明內(nèi)容】

[0003]本發(fā)明所要解決的技術(shù)問題是提供一種碼垛機(jī)器人弧形運(yùn)動(dòng)軌跡算法，減少機(jī)器人運(yùn)動(dòng)路程，使其減少每個(gè)周期的運(yùn)動(dòng)時(shí)間，同時(shí)使其運(yùn)動(dòng)過程更加柔性靈活，并通過減少運(yùn)行時(shí)間降低電力使用成本。
[0004]為解決上述技術(shù)問題，本發(fā)明的技術(shù)方案是:一種碼垛機(jī)器人弧形運(yùn)動(dòng)軌跡算法，包括以下步驟:
(1)設(shè)定碼垛機(jī)器人當(dāng)前位置的坐標(biāo)Pl(Xl，yl，zl)，目標(biāo)位置的坐標(biāo)P3 (x3,y3,z3),過渡點(diǎn)的坐標(biāo)P2 (X2，y2，z2);坐標(biāo)點(diǎn)Pl為弧形運(yùn)動(dòng)軌跡的起點(diǎn)，坐標(biāo)點(diǎn)P3為弧形運(yùn)動(dòng)軌跡的終點(diǎn)，坐標(biāo)點(diǎn)P2 (x2, y2, z2)為坐標(biāo)點(diǎn)Pl到坐標(biāo)點(diǎn)P3的過渡點(diǎn)；坐標(biāo)點(diǎn)P2到達(dá)由坐標(biāo)點(diǎn)P1、P3形成的直線的垂直高度為H ；
(2)獲取碼垛機(jī)器人當(dāng)前位置的坐標(biāo)Pl(xl, yl, zl)和目標(biāo)位置的坐標(biāo)P3 (x3, y3,
z3)；
(3)碼垛機(jī)器人讀取輸入的高度參數(shù)H，并將當(dāng)前位置坐標(biāo)Pl和目標(biāo)位置坐標(biāo)P3轉(zhuǎn)化為世界坐標(biāo)；
(4)將轉(zhuǎn)化后的當(dāng)前位置坐標(biāo)P1、目標(biāo)位置坐標(biāo)P3和高度參數(shù)H代入底層算法函數(shù)中求出過渡坐標(biāo)P2 ；
(5)將求取的過渡坐標(biāo)P2轉(zhuǎn)化為世界坐標(biāo)系下坐標(biāo)，并通過接口發(fā)送到應(yīng)用層；
(6)在碼垛機(jī)器人的用戶應(yīng)用層使用Circle指令運(yùn)行規(guī)劃圓弧運(yùn)動(dòng)軌跡。
[0005]由三維空間坐標(biāo)點(diǎn)可以確定一個(gè)平面，在這個(gè)平面上規(guī)劃一條圓弧曲線。例如Pl(xl，yl, zl), P2 (x2, y2，z2)，P3 (x3, y3，z3)，如果已知三個(gè)點(diǎn)的坐標(biāo)，只要三個(gè)點(diǎn)不在一條直線上則可以算法同時(shí)經(jīng)過三個(gè)點(diǎn)的空間圓弧，P2點(diǎn)為過渡點(diǎn)，Pl為起始點(diǎn)，P3點(diǎn)為終止點(diǎn)，圓弧軌跡的運(yùn)動(dòng)方向由Pl經(jīng)過P2到P3，角度根據(jù)三點(diǎn)形成的圓弧圓心到Pl和P3的單位向量點(diǎn)乘求得?，F(xiàn)場(chǎng)使用碼垛機(jī)器人時(shí)抓取點(diǎn)Pl已知，放置點(diǎn)P3已知，現(xiàn)在要根據(jù)P2到達(dá)P1P3形成的直線的垂直高度H求取P2點(diǎn)的坐標(biāo)。然后根據(jù)Pl，P2，P3點(diǎn)的坐標(biāo)規(guī)劃拋物線軌跡，使用機(jī)器人的Circle指令運(yùn)行規(guī)劃的軌跡。
[0006]作為改進(jìn),所述步驟(4)中，底層算法函數(shù)funct1n P2=FuncSolve ( PI, P3, h)，根據(jù)Pl，P3點(diǎn)求出中間的坐標(biāo):dist= (Pl+P3)/2 ;dist包含三個(gè)元素dist (I) X軸坐標(biāo)，dist (2) Y軸坐標(biāo)，dist (3) Z軸坐標(biāo)；碼垛機(jī)器人第四軸垂直于水平面，TCP運(yùn)行的拋物線軌跡垂直于XOY平面，假設(shè)垂直于此平面的一個(gè)方向量是(l，k，0)，則k=( Pl(I)-P3(l) ) / ( P3(2) - Pl (2));然后再根據(jù)圓弧曲線在XOY平面投影的距離求取P2⑵即中間點(diǎn)Y軸的坐標(biāo)；具體公式如下:
fun = @ (y) (point3 (I)+k*point3(2)-k*y_dist(I))'2+(y-dist(2))'2+...(((point3 (I)+k*point3(2)-k*y_dist(I)) *(point3(I)-pointI(I)) + (y-dist(2)) * (point3 (2)-pointI(2)))/(point3(3)-pointI(3)))'2-high'2P2 (2) = FSOLVE(fun,I)
其中FSOLVE是求變量為y，fun=l時(shí)y的值；
然后根據(jù)已知k，P2(2)的值求取P2(l)的值，公式如下:
P2(I) = point3(I)+k*point3(2)_k*Y最后求出P2(3):
P2 (3) =dist (3) - ((point3 (I) +k*point3 (2) -k*Y_dist (I)) * (point3 (I) -pointl (I)) +(Y-dist(2)) *(point3(2)-pointl(2)))/(point3(3)-pointl(3))。
[0007]作為改進(jìn)，所述算法應(yīng)用于平行桿碼垛機(jī)器人，如公開號(hào)為103010764A的一種平行桿碼垛機(jī)機(jī)器人。
[0008]本發(fā)明與現(xiàn)有技術(shù)相比所帶來的有益效果是:
將碼垛機(jī)器人的運(yùn)動(dòng)軌跡規(guī)劃呈拋物線，減少機(jī)器人運(yùn)動(dòng)路程，使其減少每個(gè)周期的運(yùn)動(dòng)時(shí)間，同時(shí)使其運(yùn)動(dòng)過程更加柔性靈活，并通過減少運(yùn)行時(shí)間降低電力使用成本。
【附圖說明】
[0009]圖1為現(xiàn)有碼垛機(jī)器人運(yùn)動(dòng)軌跡。
[0010]圖2為本發(fā)明碼垛機(jī)器人運(yùn)動(dòng)軌跡。
[0011]圖3為本發(fā)明算法流程圖。
【具體實(shí)施方式】
[0012]下面結(jié)合說明書附圖對(duì)本發(fā)明作進(jìn)一步說明。
[0013]如圖2、3所示，一種碼垛機(jī)器人弧形運(yùn)動(dòng)軌跡算法，該碼垛機(jī)器人為平行桿碼垛機(jī)器人，算法包括以下步驟:
(1)設(shè)定碼垛機(jī)器人當(dāng)前位置的坐標(biāo)Pl(Xl，yl，zl)，目標(biāo)位置的坐標(biāo)P3 (x3,y3,z3),過渡點(diǎn)的坐標(biāo)P2 (X2，y2，z2);坐標(biāo)點(diǎn)Pl為弧形運(yùn)動(dòng)軌跡的起點(diǎn)，坐標(biāo)點(diǎn)P3為弧形運(yùn)動(dòng)軌跡的終點(diǎn)，坐標(biāo)點(diǎn)P2 (x2, y2, z2)為坐標(biāo)點(diǎn)Pl到坐標(biāo)點(diǎn)P3的過渡點(diǎn)；坐標(biāo)點(diǎn)P2到達(dá)由坐標(biāo)點(diǎn)P1、P3形成的直線的垂直高度為H ；
(2)獲取碼垛機(jī)器人當(dāng)前位置的坐標(biāo)Pl(xl, yl, zl)和目標(biāo)位置的坐標(biāo)P3 (x3, y3,
z3)；
(3)碼垛機(jī)器人讀取輸入的高度參數(shù)H，并將當(dāng)前位置坐標(biāo)Pl和目標(biāo)位置坐標(biāo)P3轉(zhuǎn)化為世界坐標(biāo)； (4)將轉(zhuǎn)化后的當(dāng)前位置坐標(biāo)P1、目標(biāo)位置坐標(biāo)P3和高度參數(shù)H代入底層算法函數(shù)中求出過渡坐標(biāo)P2 ；
(5)將求取的過渡坐標(biāo)P2轉(zhuǎn)化為世界坐標(biāo)系下坐標(biāo)，并通過接口發(fā)送到應(yīng)用層；
(6)在碼垛機(jī)器人的用戶應(yīng)用層使用Circle指令運(yùn)行規(guī)劃圓弧運(yùn)動(dòng)軌跡。
[0014] 底層算法函數(shù)funct1n P2=FuncSolve ( Pl, P3, h ),根據(jù)Pl, P3點(diǎn)求出中間的坐標(biāo):dist= (P1+P3)/2 ;dist包含三個(gè)元素dist (I) X軸坐標(biāo)，dist (2) Y軸坐標(biāo)，dist
(3)Z軸坐標(biāo)；碼垛機(jī)器人第四軸垂直于水平面，TCP運(yùn)行的拋物線軌跡垂直于XOY平面，假設(shè)垂直于此平面的一個(gè)方向量是(1，k，0)，則k=( Pl(I) - P3(l) ) / ( P3(2) - Pl (2));然后再根據(jù)圓弧曲線在XOY平面投影的距離求取P2 (2)即中間點(diǎn)Y軸的坐標(biāo)；具體公式如下:
fun = @ (y) (point3 (I)+k*point3(2)-k*y_dist(I))'2+(y-dist(2))'2+...(((point3 (I)+k*point3(2)-k*y_dist(I)) *(point3(I)-pointl(I)) + (y-dist(2)) * (point3 (2)-pointl(2)))/(point3(3)-pointl(3)))'2-high'2P2 (2) = FSOLVE(fun,I)
其中FSOLVE是求變量為y，fun=l時(shí)y的值；
然后根據(jù)已知k，P2(2)的值求取P2(l)的值，公式如下:
P2(I) = point3(I)+k*point3(2)_k*Y最后求出P2(3):
P2 (3) =dist (3) - ((point3 (I) +k*point3 (2) -k*Y_dist (I)) * (point3 (I) -pointl (I)) +(Y-dist(2)) *(point3(2)-pointl(2)))/(point3(3)-pointl(3))。
【主權(quán)項(xiàng)】
1.一種碼垛機(jī)器人弧形運(yùn)動(dòng)軌跡算法，其特征在于，包括以下步驟: (1)設(shè)定碼垛機(jī)器人當(dāng)前位置的坐標(biāo)Pl(Xl，yl，zl)，目標(biāo)位置的坐標(biāo)P3 (x3,y3,z3),過渡點(diǎn)的坐標(biāo)P2 (X2，y2，z2);坐標(biāo)點(diǎn)Pl為弧形運(yùn)動(dòng)軌跡的起點(diǎn)，坐標(biāo)點(diǎn)P3為弧形運(yùn)動(dòng)軌跡的終點(diǎn)，坐標(biāo)點(diǎn)P2 (x2, y2, z2)為坐標(biāo)點(diǎn)Pl到坐標(biāo)點(diǎn)P3的過渡點(diǎn)；坐標(biāo)點(diǎn)P2到達(dá)由坐標(biāo)點(diǎn)P1、P3形成的直線的垂直高度為H ； (2)獲取碼垛機(jī)器人當(dāng)前位置的坐標(biāo)Pl(xl, yl, zl)和目標(biāo)位置的坐標(biāo)P3 (x3, y3,z3)； (3)碼垛機(jī)器人讀取輸入的高度參數(shù)H，并將當(dāng)前位置坐標(biāo)Pl和目標(biāo)位置坐標(biāo)P3轉(zhuǎn)化為世界坐標(biāo)； (4)將轉(zhuǎn)化后的當(dāng)前位置坐標(biāo)P1、目標(biāo)位置坐標(biāo)P3和高度參數(shù)H代入底層算法函數(shù)中求出過渡坐標(biāo)P2 ； (5)將求取的過渡坐標(biāo)P2轉(zhuǎn)化為世界坐標(biāo)系下坐標(biāo)，并通過接口發(fā)送到應(yīng)用層； (6)在碼垛機(jī)器人的用戶應(yīng)用層使用Circle指令運(yùn)行規(guī)劃圓弧運(yùn)動(dòng)軌跡。
2.根據(jù)權(quán)利要求1所述的一種碼垛機(jī)器人弧形運(yùn)動(dòng)軌跡算法，其特征在于:所述步驟(4)中，底層算法函數(shù)funct1n P2=FuncSolve ( PI, P3, h ),根據(jù)PI, P3點(diǎn)求出中間的坐標(biāo):dist= (P1+P3) /2 ； dist包含三個(gè)元素dist (I) X軸坐標(biāo)，dist (2) Y軸坐標(biāo)，dist (3) Z軸坐標(biāo)；碼垛機(jī)器人第四軸垂直于水平面，TCP運(yùn)行的拋物線軌跡垂直于XOY平面，假設(shè)垂直于此平面的一個(gè)方向量是(l，k，0)，則k=( Pl(I) - P3(l) ) / ( P3(2) - Pl (2));然后再根據(jù)圓弧曲線在XOY平面投影的距離求取P2 (2)即中間點(diǎn)Y軸的坐標(biāo)；具體公式如下:fun = @ (y) (point3 (I)+k*point3(2)-k*y_dist(I))'2+(y-dist(2))'2+...(((point3 (I)+k*point3(2)-k*y_dist(I)) *(point3(I)-pointI(I)) + (y-dist(2)) * (point3 (2)-pointI(2)))/(point3(3)-pointI(3)))'2-high'2P2 (2) = FSOLVE(fun,I) 其中FSOLVE是求變量為y，fun=l時(shí)y的值； 然后根據(jù)已知k，P2(2)的值求取P2(l)的值，公式如下:P2(I) = point3(I)+k*point3(2)_k*Y最后求出P2(3): P2 (3) =dist (3) - ((point3 (I) +k*point3 (2) -k*Y_dist (I)) * (point3 (I) -pointl (I)) +(Y-dist(2)) *(point3(2)-pointl(2)))/(point3(3)-pointl(3))。
3.根據(jù)權(quán)利要求1所述的一種碼垛機(jī)器人弧形運(yùn)動(dòng)軌跡算法，其特征在于:所述算法應(yīng)用于平行桿碼垛機(jī)器人。
【專利摘要】一種碼垛機(jī)器人弧形運(yùn)動(dòng)軌跡算法，包括以下步驟：（2）獲取碼垛機(jī)器人當(dāng)前位置的坐標(biāo)P1（x1，y1，z1）和目標(biāo)位置的坐標(biāo)P3（x3，y3，z3）；（3）碼垛機(jī)器人讀取輸入的高度參數(shù)H，并將當(dāng)前位置坐標(biāo)P1和目標(biāo)位置坐標(biāo)P3轉(zhuǎn)化為世界坐標(biāo)；（4）將轉(zhuǎn)化后的當(dāng)前位置坐標(biāo)P1、目標(biāo)位置坐標(biāo)P3和高度參數(shù)H代入底層算法函數(shù)中求出過渡坐標(biāo)P2；（5）將求取的過渡坐標(biāo)P2轉(zhuǎn)化為世界坐標(biāo)系下坐標(biāo)，并通過接口發(fā)送到應(yīng)用層；（6）在碼垛機(jī)器人的用戶應(yīng)用層使用Circle指令運(yùn)行規(guī)劃圓弧運(yùn)動(dòng)軌跡。將碼垛機(jī)器人的運(yùn)動(dòng)軌跡規(guī)劃呈拋物線，減少機(jī)器人運(yùn)動(dòng)路程，使其減少每個(gè)周期的運(yùn)動(dòng)時(shí)間，同時(shí)使其運(yùn)動(dòng)過程更加柔性靈活。
【IPC分類】G05B13-04
【公開號(hào)】CN104570735
【申請(qǐng)?zhí)枴緾N201410806960
【發(fā)明人】劉遠(yuǎn)強(qiáng), 楊文發(fā), 郭偉越
【申請(qǐng)人】廣州市萬世德智能裝備科技有限公司, 佛山市萬世德機(jī)器人技術(shù)有限公司
【公開日】2015年4月29日
【申請(qǐng)日】2014年12月23日