[发明专利]一种六自由度机械手的位置逆解方法

摘要

本发明专利公开了一种六自由度机械手的位置逆解方法。本发明包括：通过摄像头获取圆形目标物体的图像位置，用OPENCV对图像进行处理得到中心坐标和半径信息，通过比较直接的数值方法，对机械手需要到达的目标位置进行逆解，建立数学模型求出机械手各个舵机需要转过的角度，乘以各个舵机每转过一度所对应的脉宽，得到各个舵机将目标物品送达目的地需要的脉宽值，根据脉宽值控制机械手各个关节动作，实现机械手臂的按颜色自主物品抓取。本发明计算过程简便，在本方法下实现了对机械手精确自主抓物的控制，很好地避免了传统位置逆解算法中的复杂运算和多解问题，有很好的市场应用前景。

主权项

一种六自由度机械手的位置逆解方法，包括以下步骤：初始化步骤；首先让舵机处于竖直状态，调试出各个舵机对应的脉宽；逆解步骤；然后根据角度与机械手臂长度对应的关系建立数学模型，再反解出二号舵机、三号舵机、四号舵机以及五号舵机到达目标物需要转过的角度，具体步骤为：首先通过测量知道各个舵机间的舵机臂长度；所述机械手具有零号舵机、一号舵机、二号舵机、三号舵机、四号舵机以及五号舵机；通过测量获得三号舵机与四号舵机之间的距离la，以及二号舵机与三号舵机之间的距离lc；采用摄像头采集目标物品的x、y坐标，采集回来的坐标经过上位机OPENCV处理将其转化成坐标参考物从左到右的坐标，坐标从零开始算起，中间位置坐标为x₀；求出所述二号舵机、三号舵机、四号舵机、五号舵机需要转过的角度cr2、cr3、cr4、cr5，根据三角形的正余弦和正余切定理，先算出角crD的大小和边lb的长度；z为机械手臂的抬升高度；rad为机械手从中心坐标（x₀，0）到目标物品的距离；根据三角形正切定理：tan(crD)= (3.3+z)/(rad‑4.85)；              (1）由于rad为机械手从中心坐标（x0，0）到目标物品的距离，算出其大小为：rad= sqrt((x‑x0)*(x‑x0)+y*y)；              (2）得到角crD的大小为：crD= atan((3.3+z)/(rad‑4.85))；             (3）求出了角crD的大小，根据勾股定理算出未知边lb的大小为：lb=sqrt((rad‑4.85)*(rad‑4.85)+(3.3+z)*(3.3+z))；         (4）由此得到la、lb和lc的大小，跟据三角形余弦和反余弦定理得到三角形任意一个角的大小，把三角形的三个内角设为crA、crB、 crC，由余弦定理得：cos(crA)= (lb*lb+lc*lc‑la*la)/(2*lb*lc);                     (5）cos(crB)= (la*la+lc*lc‑lb*lb)/(2*la*lc);                     (6）cos(crC)= (la*la+lb*lb‑lc*lc)/(2*la*lb);                     (7）再根据反余弦定理，得出三角形各个角的大小为：crA=acos((lb*lb+lc*lc‑la*la)/(2*lb*lc))；                     (8）crB=acos((la*la+lc*lc‑lb*lb)/(2*la*lc))；                     (9）crC=acos((la*la+lb*lb‑lc*lc)/(2*la*lb))；                     (10）公式（8）（9）（10）算出了三角形各个内角的大小，再由舵机转过角度与各个角的关系算出舵机转过角度的大小；设定二号舵机转过的角度为cr2，三号舵机转过的角度为cr3，四号舵机转过的角度为cr4，五号舵机转过的角度为cr5，其计算公式如下所示：cr2=90+crD‑crA；             （11）cr3=180‑crB；                （12）cr4=90‑crC‑crD；             （13）所述crD指的是二号舵机与四号舵机之间的连线与水平方向的夹角，所述lb指的是二号舵机与四号舵机之间的距离；通过算式（11）（12）（13）得到所述二号舵机、三号舵机、四号舵机以及五号舵机到达目标物需要转过的角度；转换步骤；将所述二号舵机、三号舵机、四号舵机以及五号舵机到达目标物需要转过的角度转换成相应的脉宽传给舵机控制板，控制机械手进行自主抓物。