碼垛機械臂末端的運動軌跡規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及機器人運動規(guī)劃領(lǐng)域，尤其設(shè)及一種碼煤機械臂末端的運動軌跡規(guī)劃 方法。
【背景技術(shù)】
[0002] 碼煤機械臂末端的運功軌跡根據(jù)工作要求不同而不同，W生產(chǎn)線上產(chǎn)品卸煤堆煤 為例，工業(yè)生產(chǎn)中，需將流水線上的產(chǎn)品整齊的碼放在托盤或推車上，運一動作簡單而又重 復，可用碼煤機械臂完成。
[0003] 碼煤機械臂在運動過程中，其運動軌跡具有重復性，通過有效的軌跡規(guī)劃可W保 證運行過程中的平穩(wěn)和時間要求，進而可W提高碼煤機械臂的運行效率和穩(wěn)定性。

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

[0004] 本發(fā)明的目的在于針對現(xiàn)有技術(shù)的不足，提供一種碼煤機械臂末端的運動軌跡規(guī) 劃方法，通過用曲線段運動軌跡替代直線段運動軌跡，可W提高碼煤機械臂的運行效率和 穩(wěn)定性。
[0005] 本發(fā)明采用如下的技術(shù)方案：
[0006] 1)確定關(guān)鍵點：W機械臂所要抓取的物品的原始位置為抓取位置Pi，其在世界坐 標系中的坐標Pl(Xl，yl，Zl);W物品目標位置為放置位置P6，其坐標P6(X2，y2，Z2);
[0007] 抓取準備位置P2與抓取位置P1垂直分布，高度差為H1;移動開始位置P3與抓取準 備位置P2水平分布，距離為L1;放置準備位置Pe與放置位置Pe垂直分布，高度差為H2;移動 結(jié)束位置P4與放置準備位置P冰平分布，距離為L2;高度H1、&和距離Li、L2由具體工作要 求給定；
[000引因此，其余各關(guān)鍵點坐標分別為：P2(Xi，yi，Zi+Hi)，P3(Xi+Li，yi，Zi+Hi)，P4(X2-L2,72， Z2+H2) , 口5(而，72,Z2+H2);
[000引。確定初步運動軌跡：將上述各關(guān)鍵點依次連接獲得直線段PA、P2P3、P3P4、P4P5和PsPe，WP1P2和P2P3作為第一直角過渡段，P3P4作為中間直線段，P4P5和PsPe作為第二直 角過渡段，獲得從Pi到Pe的初始運動軌跡；
[0010]如增加控制點擬直線段PA的中間位置為控制點Ci，坐標為Ci(Xi，yi，Zi+Hi/。； P5P6的中間位置為控制點C4,坐標為C4(而，y2,Z2+H2/2);將控制點C2、C3設(shè)于中間直線段P3P4 上，口3〔2=〇3口4=1^^口4，1^為比例系數(shù)，0《1^《1;
[0011] 因此，控制點C2、C3的坐標分別為：
[0012] 〔2(Xi+Li+k(Xi+Li) -(Xz-Lz) ,y1+kyi_y2,Z1+Hi+k(Z1+Hi)- (Z2+恥）） C3(x；j-L2-k| (Xi+Li)-(x；j-L2)I，y；；-k|yi-y2,Zz+Hz-kl(Zi+Hi)-(Z2+H2)I);
[001引 4)曲線插補：WPi、Cl、PzA、Cz作為控制點，采用NURBS曲線插補理論算法，得到 經(jīng)過首末控制點Pi和C2的平滑曲線，W該曲線取代第一直角過渡段及P3C2直線段；WC3、 ?4、Ps、C4、Pe作為控制點，采用NURBS曲線插補理論算法，得到經(jīng)過首末控制點C3和Pe的平 滑曲線，w此曲線軌跡取代C3P4直線段及第二直角過渡段；獲得機械臂末端的最終運動軌 跡。
[0014] 步驟4)中所述的采用NURBS曲線插補理論算法，得到經(jīng)過首末控制點的平滑曲 線，具體方法為：
[001引一、計算節(jié)點向量
[001引已知m+p+1個控制點巧乂;立巧中m為所生成樣條的階數(shù)，P為所構(gòu)造的基函數(shù)次 數(shù)；控制點之間距離
，其中Xj表 示控制點P，的橫軸坐標，y，表示控制點P，的縱軸坐標。
[0017] 根據(jù)哈德利一賈德方法遞推公式可得：
[0018]
[0019] 其中Ui表示第i個節(jié)點；根據(jù)上式可得節(jié)點向量U遞推公式為：
[0020]
[0021] 求出的節(jié)點向量U的形式為：
[0022]
[002引二根據(jù)節(jié)點向量計算NURBS基函數(shù)
[0024] 用下列遞推方式確定基函數(shù)Ni,p(u)，即相應(yīng)于節(jié)點向量U的P次NURBS基函數(shù)：
[00巧]
[0026] 其中i是基函數(shù)的序列號；
[0027] S、得到NURBS曲線表達式，根據(jù)控制點計算出插補點 [002引第i段NURBS曲線Pi(U)的表達式為：
[0029]
[0030] 參數(shù)UG[Ui+p，UiwJ均勻變化得到插補點數(shù)據(jù)。
[0031] 本發(fā)明的有益效果在于：本發(fā)明的方法利用笛卡爾空間坐標系對碼煤機械臂進行 軌跡規(guī)劃，根據(jù)具體工作要求給定關(guān)鍵控制點坐標，運用NURBS插補理論進行曲線插補，得 到相應(yīng)平滑曲線，通過用曲線段運動軌跡替代直線段運動軌跡，最終獲得機械臂末端的運 動軌跡，本發(fā)明采用用曲線段運動軌跡可提高碼煤機械臂的運行效率和穩(wěn)定性。
【附圖說明】
[0032]圖1是本發(fā)明的碼煤機械臂末端的運動軌跡規(guī)劃方法流程。
[0033] 圖2是碼煤機械臂末端的初步運動軌跡；(a)向低處放置物品的情況；化）向高處 放置物品的情況。
[0034] 圖3是運用NURBS插補理論進行曲線插補的具體流程。
[0035] 圖4是最終規(guī)劃獲得的碼煤機械臂末端的運動軌跡；(a)向低處放置物品的情況； 化）向高處放置物品的情況。
【具體實施方式】
[0036] 下面結(jié)合附圖對本發(fā)明做進一步說明。
[0037] 本發(fā)明的碼煤機械臂末端的運動軌跡規(guī)劃方法流程如圖1所示，規(guī)劃開始時，根 據(jù)工作要求給定關(guān)鍵點坐標，根據(jù)關(guān)鍵點坐標得到直線段運動軌跡，在首末段的直線段上 選擇控制點坐標，運用NURBS插補理論進行曲線插補，得到首末段處的曲線段運動軌跡，具 體如下：
[0038] 1、確定關(guān)鍵點坐標
[0039] 碼煤機械臂在搬運過程中，由生產(chǎn)流水線到推車存在多條運動軌跡，為實現(xiàn)物品 在推車上的整齊碼放，需要不斷調(diào)整軌跡，推車上同一層的碼放軌跡根據(jù)產(chǎn)品碼放位置而 定，不同層的碼放軌跡僅在高度上存在差異。不同的運動軌跡中均存在幾個關(guān)鍵點，包括抓 取準備位置P2、抓取位置Pi、移動開始位置P3、移動結(jié)束位置P4、放置準備位置Pe、放置位置 Pe等，抓取位置P1與抓取準備位置P2垂直分布，高度為H1，抓取準備位置P2與移動開始位 置P3水平分布，距離為L1;移動結(jié)束位置P4與放置準備位置Pe水平分布，距離為L2,放置準 備位置Pe與放置位置Pe垂直分布，高度為H2。高度Hi、&和距離L1、L2由實際工作要求提 前給定。
[0040] 對于碼放過程中不同的運動軌跡，首先獲取抓取位置Pi在世界坐標系中的坐標 Pi(xi，yi，zi)及放置位置P廟世界坐標系中的坐標Pe(X2,72,Z2);其次根據(jù)給定高度Hi和H2 的值計算抓取準備位置的坐標P2 (Xl，yi，Zi+Hi)、放置準備位置Pg的坐標P5 (而，72,Z2+H2)， 根據(jù)給定距離Li和L2的值計算移動開始位置P3的坐標P3(Xi+Li，Yi，Zi+Hi)、移動結(jié)束位置 ?4的坐標P4 (X2-L2,y2,Z2+H2)。至此運功軌跡中的幾個關(guān)鍵點坐標均已給定。
[0041] 2、確定初步運動軌跡
[0042] 根據(jù)確定的關(guān)鍵點坐標，碼煤機械臂末端的初步運動軌跡由抓取位置Pi至移動開 始位置P3的首段直線段、移動開始位置P3至移動結(jié)束位置P4的中間直線段、移動結(jié)束位置 P4至放置位置Pe的末段直線段=部分組成，如圖2所示。
[0043] 因此，碼煤機械臂末端的初步運動軌跡為直線段運動軌跡，由直線段P1P2、P2P3、 P3P4、P4P5、PsPe組成，其中直線段P化和P2P3組成第一直角過渡段，P3P4為中間直線段，直線 段PaPs和P樂e組成第二直角過渡段。
[00