本發(fā)明涉及機(jī)器人運(yùn)動規(guī)劃領(lǐng)域，尤其涉及一種碼垛機(jī)械臂末端的運(yùn)動軌跡規(guī)劃方法。

背景技術(shù)：
碼垛機(jī)械臂末端的運(yùn)功軌跡根據(jù)工作要求不同而不同，以生產(chǎn)線上產(chǎn)品卸垛堆垛為例，工業(yè)生產(chǎn)中，需將流水線上的產(chǎn)品整齊的碼放在托盤或推車上，這一動作簡單而又重復(fù)，可用碼垛機(jī)械臂完成。碼垛機(jī)械臂在運(yùn)動過程中，其運(yùn)動軌跡具有重復(fù)性，通過有效的軌跡規(guī)劃可以保證運(yùn)行過程中的平穩(wěn)和時間要求，進(jìn)而可以提高碼垛機(jī)械臂的運(yùn)行效率和穩(wěn)定性。

技術(shù)實現(xiàn)要素：
本發(fā)明的目的在于針對現(xiàn)有技術(shù)的不足，提供一種碼垛機(jī)械臂末端的運(yùn)動軌跡規(guī)劃方法，通過用曲線段運(yùn)動軌跡替代直線段運(yùn)動軌跡，可以提高碼垛機(jī)械臂的運(yùn)行效率和穩(wěn)定性。本發(fā)明采用如下的技術(shù)方案：1)確定關(guān)鍵點：以機(jī)械臂所要抓取的物品的原始位置為抓取位置P1，其在世界坐標(biāo)系中的坐標(biāo)P1(x1，y1，z1)；以物品目標(biāo)位置為放置位置P6，其坐標(biāo)P6(x2，y2，z2)；抓取準(zhǔn)備位置P2與抓取位置P1垂直分布，高度差為H1；移動開始位置P3與抓取準(zhǔn)備位置P2水平分布，距離為L1；放置準(zhǔn)備位置P5與放置位置P6垂直分布，高度差為H2；移動結(jié)束位置P4與放置準(zhǔn)備位置P5水平分布，距離為L2；高度H1、H2和距離L1、L2由具體工作要求給定；因此，其余各關(guān)鍵點坐標(biāo)分別為：P2(x1，y1，z1+H1)，P3(x1+L1，y1，z1+H1)，P4(x2-L2，y2，z2+H2)，P5(x2，y2，z2+H2)；2)確定初步運(yùn)動軌跡：將上述各關(guān)鍵點依次連接獲得直線段P1P2、P2P3、P3P4、P4P5和P5P6，以P1P2和P2P3作為第一直角過渡段，P3P4作為中間直線段，P4P5和P5P6作為第二直角過渡段，獲得從P1到P6的初始運(yùn)動軌跡；3)增加控制點：以直線段P1P2的中間位置為控制點C1，坐標(biāo)為C1(x1，y1，z1+H1/2)；P5P6的中間位置為控制點C4，坐標(biāo)為C4(x2，y2，z2+H2/2)；將控制點C2、C3設(shè)于中間直線段P3P4上，P3C2＝C3P4＝kP3P4，k為比例系數(shù)，0≤k≤1；因此，控制點C2、C3的坐標(biāo)分別為：C2(x1+L1+k|(x1+L1)-(x2-L2)|，y1+k|y1-y2|，z1+H1+k|(z1+H1)-(z2+H2)|)C3(x2-L2-k|(x1+L1)-(x2-L2)|，y2-k|y1-y2，z2+H2-k|(z1+H1)-(z2+H2)|)；4)曲線插補(bǔ)：以P1、C1、P2、P3、C2作為控制點，采用NURBS曲線插補(bǔ)理論算法，得到經(jīng)過首末控制點P1和C2的平滑曲線，以該曲線取代第一直角過渡段及P3C2直線段；以C3、P4、P5、C4、P6作為控制點，采用NURBS曲線插補(bǔ)理論算法，得到經(jīng)過首末控制點C3和P6的平滑曲線，以此曲線軌跡取代C3P4直線段及第二直角過渡段；獲得機(jī)械臂末端的最終運(yùn)動軌跡。步驟4)中所述的采用NURBS曲線插補(bǔ)理論算法，得到經(jīng)過首末控制點的平滑曲線，具體方法為：一、計算節(jié)點向量已知m+p+1個控制點其中m為所生成樣條的階數(shù)，p為所構(gòu)造的基函數(shù)次數(shù)；控制點之間距離其中xj表示控制點Pj的橫軸坐標(biāo)，yj表示控制點Pj的縱軸坐標(biāo)。根據(jù)哈德利—賈德方法遞推公式可得：其中ui表示第i個節(jié)點；根據(jù)上式可得節(jié)點向量U遞推公式為：求出的節(jié)點向量U的形式為：二、根據(jù)節(jié)點向量計算NURBS基函數(shù)用下列遞推方式確定基函數(shù)Ni，p(u)，即相應(yīng)于節(jié)點向量U的p次NURBS基函數(shù)：其中i是基函數(shù)的序列號；三、得到NURBS曲線表達(dá)式，根據(jù)控制點計算出插補(bǔ)點第i段NURBS曲線Pi(u)的表達(dá)式為：參數(shù)u∈[ui+p，ui+p+1]均勻變化得到插補(bǔ)點數(shù)據(jù)。本發(fā)明的有益效果在于：本發(fā)明的方法利用笛卡爾空間坐標(biāo)系對碼垛機(jī)械臂進(jìn)行軌跡規(guī)劃，根據(jù)具體工作要求給定關(guān)鍵控制點坐標(biāo)，運(yùn)用NURBS插補(bǔ)理論進(jìn)行曲線插補(bǔ)，得到相應(yīng)平滑曲線，通過用曲線段運(yùn)動軌跡替代直線段運(yùn)動軌跡，最終獲得機(jī)械臂末端的運(yùn)動軌跡，本發(fā)明采用用曲線段運(yùn)動軌跡可提高碼垛機(jī)械臂的運(yùn)行效率和穩(wěn)定性。附圖說明圖1是本發(fā)明的碼垛機(jī)械臂末端的運(yùn)動軌跡規(guī)劃方法流程。圖2是碼垛機(jī)械臂末端的初步運(yùn)動軌跡；(a)向低處放置物品的情況；(b)向高處放置物品的情況。圖3是運(yùn)用NURBS插補(bǔ)理論進(jìn)行曲線插補(bǔ)的具體流程。圖4是最終規(guī)劃獲得的碼垛機(jī)械臂末端的運(yùn)動軌跡；(a)向低處放置物品的情況；(b)向高處放置物品的情況。具體實施方式下面結(jié)合附圖對本發(fā)明做進(jìn)一步說明。本發(fā)明的碼垛機(jī)械臂末端的運(yùn)動軌跡規(guī)劃方法流程如圖1所示，規(guī)劃開始時，根據(jù)工作要求給定關(guān)鍵點坐標(biāo)，根據(jù)關(guān)鍵點坐標(biāo)得到直線段運(yùn)動軌跡，在首末段的直線段上選擇控制點坐標(biāo)，運(yùn)用NURBS插補(bǔ)理論進(jìn)行曲線插補(bǔ)，得到首末段處的曲線段運(yùn)動軌跡，具體如下：1、確定關(guān)鍵點坐標(biāo)碼垛機(jī)械臂在搬運(yùn)過程中，由生產(chǎn)流水線到推車存在多條運(yùn)動軌跡，為實現(xiàn)物品在推車上的整齊碼放，需要不斷調(diào)整軌跡，推車上同一層的碼放軌跡根據(jù)產(chǎn)品碼放位置而定，不同層的碼放軌跡僅在高度上存在差異。不同的運(yùn)動軌跡中均存在幾個關(guān)鍵點，包括抓取準(zhǔn)備位置P2、抓取位置P1、移動開始位置P3、移動結(jié)束位置P4、放置準(zhǔn)備位置P5、放置位置P6等，抓取位置P1與抓取準(zhǔn)備位置P2垂直分布，高度為H1，抓取準(zhǔn)備位置P2與移動開始位置P3水平分布，距離為L1；移動結(jié)束位置P4與放置準(zhǔn)備位置P5水平分布，距離為L2，放置準(zhǔn)備位置P5與放置位置P6垂直分布，高度為H2。高度H1、H2和距離L1、L2由實際工作要求提前給定。對于碼放過程中不同的運(yùn)動軌跡，首先獲取抓取位置P1在世界坐標(biāo)系中的坐標(biāo)P1(x1，y1，z1)及放置位置P6在世界坐標(biāo)系中的坐標(biāo)P6(x2，y2，z2)；其次根據(jù)給定高度H1和H2的值計算抓取準(zhǔn)備位置P2的坐標(biāo)P2(x1，y1，z1+H1)、放置準(zhǔn)備位置P5的坐標(biāo)P5(x2，y2，z2+H2)，根據(jù)給定距離L1和L2的值計算移動開始位置P3的坐標(biāo)P3(x1+L1，y1，z1+H1)、移動結(jié)束位置P4的坐標(biāo)P4(x2-L2，y2，z2+H2)。至此運(yùn)功軌跡中的幾個關(guān)鍵點坐標(biāo)均已給定。2、確定初步運(yùn)動軌跡根據(jù)確定的關(guān)鍵點坐標(biāo)，碼垛機(jī)械臂末端的初步運(yùn)動軌跡由抓取位置P1至移動開始位置P3的首段直線段、移動開始位置P3至移動結(jié)束位置P4的中間直線段、移動結(jié)束位置P4至放置位置P6的末段直線段三部分組成，如圖2所示。因此，碼垛機(jī)械臂末端的初步運(yùn)動軌跡為直線段運(yùn)動軌跡，由直線段P1P2、P2P3、P3P4、P4P5、P5P6組成，其中直線段P1P2和P2P3組成第一直角過渡段，P3P4為中間直線段，直線段P4P5和P5P6組成第二直角過渡段。3、增加控制點坐標(biāo)物品由抓取位置P1至移動開始位置P3的首段直線段和由移動結(jié)束位置P4至放置位置P6的末段直線段軌跡均存在直角過渡情況，為使用NURBS插補(bǔ)理論得到首末段的曲線段運(yùn)動軌跡，除將現(xiàn)有的關(guān)鍵點坐標(biāo)作為控制點外，還需額外增加控制點坐標(biāo)。在已給定的6個控制點坐標(biāo)的基礎(chǔ)上，額外增加控制點C1、C2、C3、C4?？刂泣cC1位于直線段P1P2的中間位置，坐標(biāo)為C1(x1，y1，z1+H1/2)；控制點C2、C3位于中間直線段P3P4上，P3C2＝C3P4＝kP3P4，k為比例系數(shù)，0≤k≤1，根據(jù)比例系數(shù)k的值，可計算得到控制點C2的坐標(biāo)：C2(x1+L1+k|(x1+L1)-(x2-L2)|，y1+k|y1-y2|，z1+H1+k|(z1+H1)-(z2+H2)|)、控制點C3的坐標(biāo)：C3(x2-L2-k|(x1+L1)-(x2-L2)|，y2-k|y1-y2|，z2+H2-k|(z1+H1)-(z2+H2)|)；控制點C4位于P5P6的中間位置，坐標(biāo)為C4(x2，y2，z2+H2/2)。4、運(yùn)用NURBS插補(bǔ)理論進(jìn)行曲線插補(bǔ)由抓取位置P1至移動開始位置P3的首段運(yùn)動軌跡為直線段，為得到平滑的曲線段運(yùn)動軌跡，運(yùn)用NURBS曲線插補(bǔ)理論進(jìn)行曲線插補(bǔ)。給定控制點P1、C1、P2、P3、C2，采用NURBS曲線插補(bǔ)算法，得到一條經(jīng)過首末控制點P1和C2的平滑曲線，以此曲線軌跡取代原直線段運(yùn)動軌跡。移動開始位置P3至移動結(jié)束位置P4的軌跡采用直線段，如此可縮短軌跡位移。軌跡插補(bǔ)過程中，可通過調(diào)整比例系數(shù)k的值，以保證曲線段與直線段連接處的平滑。由移動結(jié)束位置P4到放置位置P6的末段運(yùn)動軌跡為直線段，同樣地，采用NURBS插補(bǔ)算法，給定控制點C3、P4、P5、C4、P6，得到一條經(jīng)過首末控制點C3和P6的平滑曲線，并以此曲線軌跡取代原直線段運(yùn)動軌跡。NURBS曲線插補(bǔ)的流程如圖3所示，具體過程如下。第一步：計算節(jié)點向量已知m+p+1個控制點其中m為所生成樣條階數(shù)，p為所構(gòu)造的基函數(shù)次數(shù)。控制點之間距離其中xj表示控制點Pj的橫軸坐標(biāo)，yj表示控制點Pj的縱軸坐標(biāo)。根據(jù)哈德利—賈德方法遞推公式可得：其中ui表示第i個節(jié)點。根據(jù)上式可得節(jié)點向量U遞推公式為：求出的節(jié)點向量U的形式為：第二步：根據(jù)節(jié)點向量計算NURBS基函數(shù)利用節(jié)點向量求得基函數(shù)，NURBS基函數(shù)有很多種構(gòu)造形式，一般常用的構(gòu)造形式是由如下遞推公式給出的，用下列遞推方式確定的基函數(shù)Ni，p(u)稱為相應(yīng)于節(jié)點向量U的p次NURBS基函數(shù)：其中i是基函數(shù)的序列號，給定節(jié)點向量U，根據(jù)上面的遞推公式就可以推導(dǎo)出所需要的基函數(shù)。第三步：得到NURBS曲線表達(dá)式，根據(jù)控制點計算出插補(bǔ)點第i段NURBS曲線Pi(u)的表達(dá)式為：參數(shù)u∈[ui+p，ui+p+1]均勻變化得到插補(bǔ)點數(shù)據(jù)。5、得到曲線段運(yùn)動軌跡通過NURBS曲線插補(bǔ)算法后，由抓取位置P1至C2的首段直線段和由C3至P6間的末段直線段軌跡均由獲得的相應(yīng)平滑曲線段代替。經(jīng)過NURBS曲線插補(bǔ)算法得到的曲線段運(yùn)動軌跡如圖4所示。曲線段運(yùn)功軌跡由首末段曲線段和中間直線段三部分組成。相對初步運(yùn)動軌跡的直線段運(yùn)動，曲線段運(yùn)動軌跡可以提高碼垛機(jī)械臂運(yùn)動的平穩(wěn)性。