[發明專利]一種5關節機器人末端工具位姿控制方法有效
| 申請號： | 201310378680.6 | 申請日： | 2013-08-27 |
| 公開（公告）號： | CN103481288A | 公開（公告）日： | 2014-01-01 |
| 發明（設計）人： | 高峰 | 申請（專利權）人： | 浙江工業大學 |
| 主分類號： | B25J13/00 | 分類號： | B25J13/00 |
| 代理公司： | 杭州天正專利事務所有限公司 33201 | 代理人： | 王兵;黃美娟 |
| 地址： | 310014 浙*** | 國省代碼： | 浙江;33 |
| 權利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關鍵詞： | 一種 關節 機器人 末端 工具 控制 方法 | ||
1.5旋轉關節機器人末端工具位姿控制方法，其特征在于：將圖1所示的5R（5旋轉關節）機器人看做是圖2所示的并且具有表1所示D-H參數的6R（6旋轉關節）機器人中令關節6角度θ6固定為0的特例，在其直角坐標工作空間內任意給定末端工具工作點位置（px、py、pz）和姿態角（α、β、γ），這種5R機器人的各關節角度的控制值計算過程如下：
表1：6關節機器人的D-H參數表
（1）根據任意給定的機器人末端工具工作點位置（px、py、pz）和姿態角（α、β、γ），按照已知的公式其位姿矩陣T可按下式算出：
式中：γ、β、α——機器人末端工具分別為繞固連于機器人底座的直角固定坐標軸X、Y、Z旋轉的末端工具方位角；
px、py、pz——機器人末端工具工作點在X、Y、Z軸上的坐標值。
cα、cβ、cγ——分別代表α、β、γ的余弦；
sα、sβ、sγ——分別代表α、β、γ的正弦；
（2）求出T后，根據已知公式計算圖2所示6R機器人關節角度θ1、θ2、θ3如下：
式中si、ci——分別代表sin(θi)、cos(θi)，i=1～6；
atan——反正切函數。
式（2）中的其他參數見下式：
（3）求出T、θ1、θ2、θ3后，按下式計算θ4：
式中：sij、cij——分別代表sin(θi+θj)、cos(θi+θj)，i,j=1～6；
rij——位姿矩陣元素，i,j=1～3，見式（1）；
（4）求出θ4后，再按下式計算γ′：
（5）用求出的γ′，與給定的（px、py、pz）和姿態角α、β組成新的位姿（α、β、γ′、px、py、pz），這是使圖2所示的6R機器人中關節角θ6等于0的位姿；
（6）根據新的位姿參數（α、β、γ′、px、py、pz），按照下式重新計算機器人位姿矩陣T′：
式中，由于位姿矩陣元素r11、r21、r31只和姿態角α、β相關，因此在α、β不變的前提下，r11、r21、r31也不變，見式（1）。
（7）根據上面得出的T′，利用6R機器人逆運動學公式，按下式計算相應的關節角θ5、θ6的正弦和余弦值：
進而用下式求出節角θ5、θ6：
其中θ6的計算結果將始終為0。
（8）將上面步驟（2）、步驟（3）、步驟（7）計算出的θ1、θ2、θ3、θ4、θ5作為目標值，分別應用于圖1所示5R機器人關節1～關節5的角度控制，得到的末端工具姿態和位置即為（α、β、γ′、px、py、pz），與初始給定的5R機器人末端工具姿態和位置（α、β、γ、px、py、pz）相比，工作點位置（px、py、pz）沒有變化，工作姿態中只有γ角發生了變化。
該專利技術資料僅供研究查看技術是否侵權等信息，商用須獲得專利權人授權。該專利全部權利屬于浙江工業大學，未經浙江工業大學許可，擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作，請聯系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/201310378680.6/1.html，轉載請聲明來源鉆瓜專利網。
