[0056] 獲取曲線段運(yùn)動軌跡坐標(biāo)和反解得到各臂轉(zhuǎn)動角度的過程由工控機(jī)完成，工控機(jī) 與運(yùn)動控制卡通過PCI總線進(jìn)行通訊。
[0057] 具體過程如下：
[005引1)獲取機(jī)械臂末端運(yùn)動軌跡曲線，如圖3給出了兩種運(yùn)動軌跡曲線，a)為向低處 放置物品，b)為向高處放置物品；在該運(yùn)動軌跡曲線上，連續(xù)選取包含起始點(diǎn)和終止點(diǎn)的n 個坐標(biāo)點(diǎn)，記為Pi (Xi, y;，Zi)，其中i = 1，…，n ;
[005引。對選取的每一坐標(biāo)點(diǎn)Pi(Xi,y;，Zi)進(jìn)行運(yùn)動學(xué)反解計算，得到該坐標(biāo)點(diǎn)對應(yīng)的 各驅(qū)動關(guān)節(jié)處的轉(zhuǎn)動角度信息；從而獲得運(yùn)動軌跡曲線與各臂轉(zhuǎn)動角度之間的對應(yīng)關(guān)系；
[0060] 碼煤機(jī)械臂的運(yùn)動反解依托于經(jīng)規(guī)劃得到的曲線段運(yùn)動軌跡。對得到的曲線段運(yùn) 動軌跡進(jìn)行運(yùn)動學(xué)反解計算，得到各驅(qū)動關(guān)節(jié)處的轉(zhuǎn)動角度。如圖4所示，具體過程如下：
[0061] 考慮到碼煤機(jī)械臂的工作特征，即臂4在工作過程中始終保持與地面平行，故臂 4的坐標(biāo)系{4}和末端執(zhí)行器5的坐標(biāo)系（引始終在同一平面上，因此坐標(biāo)系{4}的坐標(biāo) (Xi4，yi4，Zi4)和坐標(biāo)系巧}的坐標(biāo)（Xi5，yi5，Zi5)之間存在如下的對應(yīng)關(guān)系：
[0062]
[0063] 如此，可先將末端運(yùn)動軌跡由坐標(biāo)系間移動到坐標(biāo)系{4}，在此基礎(chǔ)上進(jìn)行運(yùn)動 學(xué)分析。
[0064] 對末端軌跡坐標(biāo)點(diǎn)Pl(Xl，yl，Zl)，即坐標(biāo)系{引的坐標(biāo)為（Xl，yl，Zl)的點(diǎn)，根據(jù)巧） 式可知，其在坐標(biāo)系{4}中的坐標(biāo)為
[0065] (Xi+l4sin白 1,Yi-lACOs白 1,Zi)。
[0066] 由
其中 tana=Yi/xi，求解得到坐標(biāo)點(diǎn)Pi(Xi,yi,Zi)相應(yīng)的臂1的轉(zhuǎn)動角度01;
[0067] 由
求解得 到坐標(biāo)點(diǎn)Pi(Xi，Yi，Zi)相應(yīng)的臂3的轉(zhuǎn)動角度0 3;
[0068]由
[0069] 其中tar
求解得到坐標(biāo)點(diǎn)Pi(Xi，yi，Zi)相應(yīng)的臂2的轉(zhuǎn)動 角度0 2;
[0070] 根據(jù)0 4= 0 3- 0 2,得到坐標(biāo)點(diǎn)Pi(Xi，yi，Zi)相應(yīng)的臂4的轉(zhuǎn)動角度0 4;由
其中
，求解得到坐標(biāo)點(diǎn)Pi(Xi,yi,Zi)相應(yīng)的臂5的轉(zhuǎn)動角 度05。
[0071] 對每個選取的點(diǎn)做運(yùn)動學(xué)反解，獲得運(yùn)動軌跡曲線與各臂轉(zhuǎn)動角度之間的對應(yīng)關(guān) 系。
[0072] 3)將上述對應(yīng)關(guān)系下發(fā)至運(yùn)動控制卡，通過伺服驅(qū)動系統(tǒng)驅(qū)動各臂轉(zhuǎn)動，通過傳 感器檢測并反饋轉(zhuǎn)動信號，各臂聯(lián)動完成給定的運(yùn)動軌跡。
[0073] 具體可采用六軸運(yùn)動控制卡，實現(xiàn)對五臺伺服直流電機(jī)的位置控制。運(yùn)動控制卡 與工控機(jī)間采用PCI總線進(jìn)行數(shù)據(jù)傳輸。工控機(jī)完成運(yùn)動軌跡坐標(biāo)反解后，將得到的各臂 轉(zhuǎn)動角度信號下發(fā)給運(yùn)動控制卡，經(jīng)運(yùn)動控制卡處理后，生成伺服直流電機(jī)驅(qū)動信號，通過 控制伺服驅(qū)動器進(jìn)而控制電機(jī)的轉(zhuǎn)動角度和速度。
[0074] 伺服驅(qū)動器接受來自運(yùn)動控制卡的控制信號，并按照控制指令完成對伺服直流電 機(jī)轉(zhuǎn)動角度和速度的控制。
[00巧]傳感器用于檢測各臂轉(zhuǎn)動角度信號，并將信號反饋給運(yùn)動控制卡，實現(xiàn)對軌跡的 精確控制。
[0076] 在伺服直流電機(jī)的作用下，各臂聯(lián)動，使末端走出給定的運(yùn)動軌跡。
【主權(quán)項】
1. 一種五自由度旋轉(zhuǎn)鏈?zhǔn)酱a垛機(jī)械臂的運(yùn)動控制方法，其特征在于，包括如下步驟： 1) 獲取機(jī)械臂末端運(yùn)動軌跡曲線，在該運(yùn)動軌跡曲線上，連續(xù)選取包含起始點(diǎn)和終止 點(diǎn)的η個坐標(biāo)點(diǎn)，記為Pi (Xi, y;，Zi)，其中i = 1，. . .，η ; 2) 對選取的每一坐標(biāo)點(diǎn)PiUi, Yi, Zi)進(jìn)行運(yùn)動學(xué)反解計算，得到該坐標(biāo)點(diǎn)對應(yīng)的各驅(qū) 動關(guān)節(jié)處的轉(zhuǎn)動角度信息；從而獲得運(yùn)動軌跡曲線與各臂轉(zhuǎn)動角度之間的對應(yīng)關(guān)系； 3) 將上述對應(yīng)關(guān)系下發(fā)至運(yùn)動控制卡，通過伺服驅(qū)動系統(tǒng)驅(qū)動各臂轉(zhuǎn)動，通過傳感器 檢測并反饋轉(zhuǎn)動信號，各臂聯(lián)動完成給定的運(yùn)動軌跡。2. 根據(jù)權(quán)利要求1所述的五自由度旋轉(zhuǎn)鏈?zhǔn)酱a垛機(jī)械臂的運(yùn)動控制方法，其特征在 于，步驟2)中所述的對坐標(biāo)點(diǎn)Pi (Xi, y;，Zi)進(jìn)行運(yùn)動學(xué)反解計算，具體包括如下步驟： 1) 將選取的坐標(biāo)AP1(^y1J1)由末端執(zhí)行器5所在的坐標(biāo)系{5}移動到 臂4所在的坐標(biāo)系{4}中，得到運(yùn)動軌跡曲線上的點(diǎn)在坐標(biāo)系{4}中相應(yīng)的坐標(biāo) ( Xl+l4sin Θ JTi-I4C0s Θ Ζι);其中I4為臂4的長度，Θ i為臂1的轉(zhuǎn)動角度； 2) 不考慮機(jī)械臂末端執(zhí)行器轉(zhuǎn)動角度時，坐標(biāo)系{4}處的齊次變換矩陣可表示為：其中，n'、〇'、a'分別為法向向量、方位向量和接近向量，用于描述臂4的姿態(tài)，其中，12、13分別為臂2和臂3的長度，Θ 2、Θ 3分別為臂2、臂3的轉(zhuǎn)動角度，d2、d3、d4 分別為臂2與1間的距離、臂3與2間的距離、臂4與3間的距離， 由⑵式可得，其中tan α =yi/xi，求解得到坐標(biāo)點(diǎn)Pi (Xi, y;，Zi)相應(yīng)的臂1的轉(zhuǎn)動角度Θ 1;3)由步驟2)中（I)、（3) 式可得，，:求解得到坐標(biāo) 點(diǎn)P1 (Xl，yi，Z1)相應(yīng)的臂3的轉(zhuǎn)動角度Θ 3; 4) 由步驟2)中⑶式可得，其中求解得到坐標(biāo)點(diǎn)Pi (Xi, yi，Zi)相應(yīng)的臂2的轉(zhuǎn)動角度θ 2; 5) 根據(jù)θ 4= θ 3- Θ 2，得到坐標(biāo)點(diǎn)Pi (Xi，yi, Zi)相應(yīng)的臂4的轉(zhuǎn)動角度Θ 4; 6) 考慮機(jī)械臂末端執(zhí)行器的轉(zhuǎn)動角度時，碼垛機(jī)械臂末端執(zhí)行器位姿的齊次變換矩陣 可表示為：其中n、〇、a分別為法向向量、方位向量和接近向量，用于描述末端執(zhí)行器5的姿態(tài)； 根據(jù)與連桿變換方程聯(lián)立，碼垛機(jī)械臂的運(yùn)動學(xué)方程可表示為得到：I 求解得到坐標(biāo)點(diǎn)Pi (Xi, yi，Zi)相應(yīng)的臂5的轉(zhuǎn)動角度θ 5。
【專利摘要】本發(fā)明公開了一種五自由度旋轉(zhuǎn)鏈?zhǔn)酱a垛機(jī)械臂的運(yùn)動控制方法，適用于全旋轉(zhuǎn)的鏈?zhǔn)綑C(jī)械臂，通過對給定的末端運(yùn)動軌跡進(jìn)行運(yùn)動學(xué)反解，得到其對應(yīng)的各驅(qū)動關(guān)節(jié)處的轉(zhuǎn)動角度信息，并下發(fā)至運(yùn)動控制卡，通過伺服驅(qū)動系統(tǒng)驅(qū)動各臂轉(zhuǎn)動，進(jìn)而完成給定的運(yùn)動軌跡，通過傳感器檢測并反饋各臂轉(zhuǎn)動角度，以提高末端控制器所走出軌跡的精度。本發(fā)明的方法針對五自由度鏈?zhǔn)酱a垛機(jī)械臂的工作特征，通過坐標(biāo)系的移動，使得反解的過程變得簡單。
【IPC分類】B25J9/16
【公開號】CN105171744
【申請?zhí)枴?br>【發(fā)明人】史偉民, 葛宏偉, 楊亮亮, 許守金
【申請人】浙江理工大學(xué)
【公開日】2015年12月23日
【申請日】2015年7月29日