一种六自由度机械手的位置逆解方法

1.一种六自由度机械手的位置逆解方法，包括以下步骤：
初始化步骤；
首先让舵机处于竖直状态，调试出各个舵机对应的脉宽；
逆解步骤；
然后根据角度与机械手臂长度对应的关系建立数学模型，再反解出二号舵机、三号舵机、四号舵机以及五号舵机到达目标物需要转过的角度，具体步骤为：
首先通过测量知道各个舵机间的舵机臂长度；
所述机械手具有零号舵机、一号舵机、二号舵机、三号舵机、四号舵机以及五号舵机；通过测量获得三号舵机与四号舵机之间的距离la，以及二号舵机与三号舵机之间的距离lc；
采用摄像头采集目标物品的x、y坐标，采集回来的坐标经过上位机OPENCV处理将其转化成坐标参考物从左到右的坐标，坐标从零开始算起，中间位置坐标为x0；求出所述二号舵机、三号舵机、四号舵机、五号舵机需要转过的角度cr2、cr3、cr4、cr5，根据三角形的正余弦和正余切定理，先算出角crD的大小和边lb的长度；z为机械手臂的抬升高度；rad为机械手从中心坐标（x0，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）得到所述二号舵机、三号舵机、四号舵机以及五号舵机到达目标物需要转过的角度；
转换步骤；
将所述二号舵机、三号舵机、四号舵机以及五号舵机到达目标物需要转过的角度转换成相应的脉宽传给舵机控制板，控制机械手进行自主抓物。
2.根据权利要求1所述的一种六自由度机械手的位置逆解方法，其特征在于：舵机控制板由舵机信号输出电路和舵机脉冲发生电路组成。
3.根据权利要求2所述的一种六自由度机械手的位置逆解方法，其特征在于：所述舵机信号输出电路对舵机的角度进行控制，通过给舵机提供相应的脉冲值，完成对舵机的角度控制。
4.根据权利要求2所述的一种六自由度机械手的位置逆解方法，其特征在于：所述舵机脉冲信号发生电路主要是通过上位机给舵机控制板发送需要产生的脉宽值，来决定产生多少脉冲，然后经过处理输送给舵机输出电路。
5.根据权利要求1至4中任一项所述的一种六自由度机械手的位置逆解方法，其特征在于：所述坐标参考物为A4纸。

一种六自由度机械手的位置逆解方法\n技术领域\n[0001] 本发明涉及一种六自由度机械手的位置逆解方法，属于机械手自动控制领域。\n背景技术\n[0002] 位置逆解问题是机械手机构学乃至机械手学中的最基础也是最重要的研究问题之一，它直接关系到机械手运动分析、离线编程、轨迹规划和实时控制等工作。因为速度和加速度分析都要在进行位置分析的基础之上才能进行，所以位置逆解问题是机械手运动规划和轨迹规划的基础，只有通过运动学逆解把空间位姿转换为关节变量，才能实现对机械手末端执行器的控制。而从工程应用的角度出发，位置逆解问题的研究成果可以很容易地应用到机械手上面，因此更加促进了对位置逆解问题的研究。\n[0003] 对于运动学正解来说，它的解是唯一确定的，即各个关节变量给定之后，机械手的末端抓手和工具的位姿是唯一确定的;而运动学反解往往具有多重解，也可能不存在解。位置逆解的复杂程度往往与机械手的结构有很大关系。由于一般情况下，六个自由度便可满足机械手在工作空间内可达任一位姿，因此六自由度机械手的位置逆解最具有研究价值和实用价值。\n[0004] 对六自由度机械手运动学的位置逆解问题，已经有很多学者作了大量的分析和研究。常用的数值方法主要包括牛顿拉夫森法、优化算法，区间算法，遗传算法和同伦算法等方法。数值方法求解一般是先建立包括若干个未知量的一个方程组，然后提供一组初始值，再利用各种优化法进行迭代，使之逐步收敛于机构的一组解。这一类数值方法的优点是求解过程比较简单，但是在计算中需要提供适当的初始值，因此涉及到初始值的选取问题；另外，这些方法不能根据方程组的情况来确定机械手机构有多少组解，也很难得到全部解。\n发明内容\n[0005] 本发明的目的在于通过提供一种直接的数值方法进行机械手臂的位置逆解，建立一个角度与长度对应关系的数学模型，采用迭代法和反解法算出各个舵机需要转过的角度，再转换成对应的脉宽，控制机械手进行自主抓物。\n[0006] 为了解决上述技术特征，现提出一种六自由度机械手的位置逆解方法，包括以下步骤：\n[0007] A.初始化步骤；\n[0008] 首先让舵机处于竖直状态，调试出各个舵机对应的脉宽；\n[0009] B.逆解步骤；\n[0010] 然后根据角度与机械手臂长度对应的关系建立数学模型，再反解出各个舵机到达目标物需要转过的角度，具体步骤为：\n[0011] 首先通过测量知道各个舵机间的舵机臂长度；\n[0012] 所述机械手具有零号舵机、一号舵机、二号舵机、三号舵机、四号舵机、五号舵机；\n通过测量获得三号舵机与四号舵机之间的距离la，以及二号舵机与三号舵机之间的距离lc；采用摄像头采集目标物品的x、y坐标，采集回来的坐标经过上位机OPENCV处理将其转化成坐标参考物从左到右的坐标，坐标从零开始算起，中间位置坐标为x0；求出各个舵机需要转过的角度cr2、cr3、cr4、cr5，根据三角形的正余弦和正余切定理，先算出角crD的大小和边lb的长度；\n[0013] 根据三角形正切定理：\n[0014] tan(crD)= (3.3+z)/(rad-4.85)；              (1）\n[0015] 由于rad为机械手从中心坐标（x0，0）到目标物品的距离，算出其大小为：\n[0016] rad= sqrt((x-x0)*(x-x0)+y*y)；              (2）\n[0017] 得到知角crD的大小为：\n[0018] crD= atan((3.3+z)/(rad-4.85))；             (3）\n[0019] 求出了角crD的大小，根据勾股定理算出未知边lb的大小为：\n[0020] lb=sqrt((rad-4.85)*(rad-4.85)+(3.3+z)*(3.3+z))；         (4）[0021] 由此得到la、lb和lc的大小，跟据三角形余弦和反余弦定理得到三角形任意一个角的大小，把三角形的三个内角设为crA、crB、 crC，由余弦定理得：\n[0022] cos(crA)= (lb*lb+lc*lc-la*la)/(2*lb*lc);                     (5）[0023] cos(crB)= (la*la+lc*lc-lb*lb)/(2*la*lc);                     (6）[0024] cos(crC)= (la*la+lb*lb-lc*lc)/(2*la*lb);                     (7）[0025] 再根据反余弦定理，得出三角形各个角的大小为：\n[0026] crA=acos((lb*lb+lc*lc-la*la)/(2*lb*lc))；                     (8）[0027] crB=acos((la*la+lc*lc-lb*lb)/(2*la*lc))；                     (9）[0028] crC=acos((la*la+lb*lb-lc*lc)/(2*la*lb))；                     (10）[0029] 公式（8）（9）（10）算出了三角形各个内角的大小，再由舵机转过角度与各个角的关系算出舵机转过角度的大小；设定二号舵机转过的角度为cr2，三号舵机转过的角度为cr3，四号舵机转过的角度为cr4，五号舵机转过的角度为cr5，其计算公式如下所示：\n[0030] cr2=90+crD-crA；             （11）\n[0031] cr3=180-crB；                （12）\n[0032] cr4=90-crC-crD；             （13）\n[0033] 通过算式（11）（12）（13）得到各个舵机到达目标物需要转过的角度；\n[0034] C. 转换步骤；\n[0035] 将各个舵机到达目标物需要转过的角度转换成相应的脉宽传给舵机控制板，控制机械手进行自主抓物。\n[0036] 作为进一步优选的技术方案，舵机控制板由舵机信号输出电路和舵机脉冲发生电路组成。\n[0037] 作为进一步优选的技术方案，所述舵机信号输出电路对舵机的角度进行控制，通过给舵机提供相应的脉冲值，完成对舵机的角度控制。\n[0038] 作为进一步优选的技术方案，所述舵机脉冲信号发生电路主要是通过上位机给舵机控制板发送需要产生的脉宽值，来决定产生多少脉冲，然后经过处理输送给舵机输出电路。\n[0039] 作为进一步优选的技术方案，所述坐标参考物为A4纸。\n[0040] 本发明的有益效果是，通过摄像头获取圆形目标物体的图像位置，用OPENCV对图像进行处理得到中心坐标和半径信息，根据中心坐标和半径对机械手臂的运动学进行分析，通过比较直接的数值方法，对机械手需要到达的目标位置进行逆解，建立数学模型求出机械手各个舵机需要转过的角度，并根据求出的解来控制机械手各个关节动作，实现机械手臂的按颜色自主物品抓取和分类。\n附图说明\n[0041] 图1是本发明的机械手数学模型；\n[0042] 图2是本发明的舵机信号输出电路；\n[0043] 图3是本发明的舵机脉冲信号发生电路；\n[0044] 图4是本发明机械手位置逆解算法程序流程；\n[0045] 图5是基于本发明所实现的机械手按颜色自主抓物实物图。\n具体实施方式\n[0046] 本发明将通过下面的实例进行更详细的描述，但是本发明不受限于以下实施例，坐标参考物也不局限于A4纸，形状规则、边缘容易区分的物品都可以用来作为坐标参考物品。\n[0047] 本发明一种六自由度机械手的位置逆解方法，提出新的控制机械手到达目标物的位置逆解方法。\n[0048] 一种六自由度机械手的位置逆解方法：从本发明建立的机械手臂数学模型中，可以知道每个舵机臂的长度，z为待给定值即机械手臂的抬升高度，rad为机械手从中心坐标（x0，0）到目标物品的距离。首先通过测量知道各个舵机间的舵机臂长度，舵机号与舵机臂对应长度关系见下表。\n[0049]\n舵机对应的序号 舵机臂长度（cm）\n0～2 14.7\n2～3（lc） 9.8\n3～4（la） 10.45\n4～5 11.4\n[0050] 根据舵机臂长度表可以知道la=10.45，lc=9.8。本发明采用摄像头采集目标物品的x、y坐标，采集回来的坐标经过上位机OPENCV处理将其转化成A4纸从左到右的坐标，坐标从零开始算起，中间位置坐标为x0。要求出各个舵机需要转过的角度cr2、cr3、cr4、cr5，根据三角形的正余弦和正余切定理，先算出角crD的大小和边lb的长度。首先三角形两边的长度分别为：rad-2.15-2.7、14.7-11.4+z，即rad-4.85、14.7-11.4+z，根据三角形正切定理可知：\n[0051] tan(crD)= (3.3+z)/(rad-4.85)；                            (1）[0052] 由于rad为机械手从中心坐标（x0，0）到目标物品的距离，可以算出其大小为：\n[0053] rad= sqrt((x-x0)*(x-x0)+y*y)；                            (2）[0054] 这样就可以得知角crD的大小为：\n[0055] crD= atan((3.3+z)/(rad-4.85))；                             (3）[0056] 求出了角crD的大小，已知本发明所构建的模型三角形是直角三角形，根据勾股定理算出未知边lb的大小为：\n[0057] lb=sqrt((rad-4.85)*(rad-4.85)+(3.3+z)*(3.3+z))；            (4）[0058] 通过上述过程知道了la、lb和lc的大小，跟据三角形余弦和反余弦定理可以得到三角形任意一个角的大小，把三角形的三个内角设为crA、crB、 crC，由余弦定理得：\n[0059] cos(crA)= (lb*lb+lc*lc-la*la)/(2*lb*lc);                   (5）[0060] cos(crB)= (la*la+lc*lc-lb*lb)/(2*la*lc);                   (6）[0061] cos(crC)= (la*la+lb*lb-lc*lc)/(2*la*lb);                   (7）[0062] 再根据反余弦定理，可以得出三角形各个角的大小为：\n[0063] crA=acos((lb*lb+lc*lc-la*la)/(2*lb*lc))；                   (8）[0064] crB=acos((la*la+lc*lc-lb*lb)/(2*la*lc))；                   (9）[0065] crC=acos((la*la+lb*lb-lc*lc)/(2*la*lb))；                   (10）[0066] 从以上的公式算出了三角形各个内角的大小，再由舵机转过角度与各个角的关系算出舵机转过角度的大小。设定二号舵机转过的角度为cr2，三号舵机转过的角度为cr3，四号舵机转过的角度为cr4，五号舵机转过的角度为cr5，其计算公式如下所示：\n[0067] cr2=90+crD-crA；                                        （11）[0068] cr3=180-crB；                                            （12）[0069] cr4=90-crC-crD；                                           （13）[0070] 五号舵机转过的角度cr5是最底下舵机的角度，其转过后角度对应的机械手的方向应指向物体。如果x-x0的误差小于万分之一时，五号舵机对应的角度为cr5=90；当x-x0大于零时，cr5=atan(y/(x-x0))；当x-x0小于零时，cr5=cr5+180。一号舵机为控制机械爪转成什么方向的舵机，只需在使用范围内转动即可，不用算出其具体需要转过的角度。零号舵机为机械爪夹东西时的张度，只要测试好张开和抓紧物品的脉宽就可以算出需要转动的角度。\n[0071] 本发明通过以上计算，已经算出了各个舵机对应的角度，下一步需要确定舵机转过的角度与舵机控制器对应脉宽的大小。首先要知道每个舵机的脉宽范围，由于每个舵机制作工艺的误差，造成了舵机的使用范围也不同，因此应该算出舵机转过每一度时对应的脉宽。由于舵机的角度范围为0～185度，其每一角度对应的脉宽y计算方法为：有效脉宽范围的量程除以角度范围的量程，各个舵机每一度对应的脉宽见下表。\n[0072]\n舵机序号 有效脉宽范围 每一度对应的脉宽\n0 500～2240 9.4054\n1 500～2320 9.8378\n2 500～2320 9.8378\n3 500～2290 9.6757\n4 500～2290 9.6757\n5 500～2230 9.3514\n[0073] 将目标物品在A4纸上的相对位置坐标代入前述公式，计算出每个舵机需要转过的具体角度值后，根据上表中各个舵机每转过一度所对应的脉宽，两者相乘，就可以算出舵机控制器需要给每个舵发送的脉宽值。上述计算过程均由下位机ARM单片机编程实现，从而达到分类抓取物品的要求。