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

说明书

技术领域

    本发明涉及一种六自由度机械手的位置逆解方法，属于机械手自动控制领域。

背景技术

位置逆解问题是机械手机构学乃至机械手学中的最基础也是最重要的研究问题之一，它直接关系到机械手运动分析、离线编程、轨迹规划和实时控制等工作。因为速度和加速度分析都要在进行位置分析的基础之上才能进行，所以位置逆解问题是机械手运动规划和轨迹规划的基础，只有通过运动学逆解把空间位姿转换为关节变量，才能实现对机械手末端执行器的控制。而从工程应用的角度出发，位置逆解问题的研究成果可以很容易地应用到机械手上面，因此更加促进了对位置逆解问题的研究。

对于运动学正解来说，它的解是唯一确定的，即各个关节变量给定之后，机械手的末端抓手和工具的位姿是唯一确定的;而运动学反解往往具有多重解，也可能不存在解。位置逆解的复杂程度往往与机械手的结构有很大关系。由于一般情况下，六个自由度便可满足机械手在工作空间内可达任一位姿，因此六自由度机械手的位置逆解最具有研究价值和实用价值。

对六自由度机械手运动学的位置逆解问题，已经有很多学者作了大量的分析和研究。常用的数值方法主要包括牛顿拉夫森法、优化算法，区间算法，遗传算法和同伦算法等方法。数值方法求解一般是先建立包括若干个未知量的一个方程组，然后提供一组初始值，再利用各种优化法进行迭代，使之逐步收敛于机构的一组解。这一类数值方法的优点是求解过程比较简单，但是在计算中需要提供适当的初始值，因此涉及到初始值的选取问题；另外，这些方法不能根据方程组的情况来确定机械手机构有多少组解，也很难得到全部解。

发明内容

    本发明的目的在于通过提供一种直接的数值方法进行机械手臂的位置逆解，建立一个角度与长度对应关系的数学模型，采用迭代法和反解法算出各个舵机需要转过的角度，再转换成对应的脉宽，控制机械手进行自主抓物。

为了解决上述技术特征，现提出一种六自由度机械手的位置逆解方法，包括以下步骤：

A.  初始化步骤；

首先让舵机处于竖直状态，调试出各个舵机对应的脉宽；

B.  逆解步骤；

然后根据角度与机械手臂长度对应的关系建立数学模型，再反解出各个舵机到达目标物需要转过的角度，具体步骤为：

首先通过测量知道各个舵机间的舵机臂长度；

所述机械手具有零号舵机、一号舵机、二号舵机、三号舵机、四号舵机、五号舵机；通过测量获得三号舵机与四号舵机之间的距离la，以及二号舵机与三号舵机之间的距离lc；采用摄像头采集目标物品的x、y坐标，采集回来的坐标经过上位机OPENCV处理将其转化成坐标参考物从左到右的坐标，坐标从零开始算起，中间位置坐标为x₀；求出各个舵机需要转过的角度cr2、cr3、cr4、cr5，根据三角形的正余弦和正余切定理，先算出角crD的大小和边lb的长度；

根据三角形正切定理：

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.5+z)/(rad-4.85))；             (3）

求出了角crD的大小，根据勾股定理算出未知边lb的大小为：

lb=sqrt((rad-4.85)*(rad-4.85)+(3.5+z)*(3.5+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）

再根据反余弦定理，得出三角形各个角的大小为：