专利名称：机械手臂反向运动方法
技术领域：
本发明涉及一种机械手臂运动方法，尤其涉及一种机械手臂反向运动方法。
背景技术：
对于改善生产力及品质而言，机械手臂的精密运动控制是非常重要的。机械手臂 在工业上的应用已经非常广泛，例如测量机台的量测手臂，机器人手臂等。然而在研究这些 机械手臂的运动中常常运用到反向动力学技术，即已知机械手臂需要到达的空间坐标点， 使得该机械手臂到达该目标位置，然而实际情况下，并不能够正好到达该目标位置，只能达 到距离目标点最近的位置，即最优位置，这就需要计算该最优位置的坐标值，目前，最常用 的方法是枚举出所有可能的方程式进行解析方程，当机械手臂的关节很多的时候，这些方 程的计算量会变得很大，导致计算变得很慢，计算结果也不是最优结果。
发明内容
鉴于以上内容，有必要提供一种机械手臂反向运动方法，可以使得机械手臂快速 到达最优的位置。一种机械手臂反向运动方法，该方法包括a)输入机械手臂初始位置的参数值和 目标位置参数值，所述机械手臂的初始位置参数值包括初始位置上每个关节的根节点和末 节点的坐标值，所述目标位置参数值包括目标位置上每个关节的末节点的坐标值；b)根据 输入的初始位置参数值和目标位置参数值，利用迭代法计算每次迭代后每个关节的末节点 所到达的位置；c)根据上述计算的每次迭代中每个关节的末节点所到达的位置，计算每次 迭代中每个关节的末节点所到达的位置与目标位置之间的距离，并得到其中最小距离的每 个关节的位置及对应该位置的每个关节末节点的坐标值；及d)每个关节上的马达驱动该 关节移动到达上述与目标位置距离最小的位置。相较于现有技术，所述机械手臂反向运动方法，利用拟牛顿迭代来计算机械手臂 的最优位置，使得机械手臂快速到达该位置。
图1是本发明机械手臂反向运动方法的较佳实施例的硬件架构图。图2是机械手臂运动的平面示意图。图3是本发明机械手臂反向运动方法较佳实施例的作业流程图。图4是图3中步骤S31和步骤S32的细化流程图。
具体实施例方式如图1所示，是本发明机械手臂反向运动方法的较佳实施例的硬件架构图。该硬 件架构图包括主机1，该主机1连接机械手臂2，本实施例中，该机械手臂2包括多个关节。 如图2所示，假设该机械手臂2的关节数目为n，每个关节包括一个根节点和一个末节点，当关节移动时，该关节的根节点的位置不变，该末节点内装有一个马达3，所述主机1发送运 动指令给所述马达3，该马达3驱使该末节点带动机械手臂2运动。从该机械手臂2连接主 机1的一端开始命名该机械手臂2的关节为第η关节，分别为第一关节，第二关节等。本 图中第一关节的根节点为Α，第一关节的末节点为B。第二关节的根节点为B，第二关节的末 节点为C。所述主机1包括一个量测单元100，该量测单元100用于测量所述机械手臂2初 始位置每个关节的根节点和末节点的坐标值。如图3所示，是本发明机械手臂反向运动方法较佳实施例的流程图。本实施例中， 以图2中所示的机械手臂2为例进行说明本发明。步骤S30，输入量测单元100所测量的机械手臂2初始位置的参数值、目标位置的 参数值及用户设置的精度值FimX。所述机械手臂2的初始位置参数值包括每个关节的根节 点和末节点的坐标值。所述机械手臂2目标位置的参数值包括每个关节的末节点的位置坐 标值。本实施例中，第一关节根节点A的初始位置坐标值(XA，\，Za)，第一关节的末节点B 初始位置坐标值为(X1B，Y1B，Z1B)，该第一关节末节点的目标位置坐标值为(X2B，Y2B，Z2B)。 所述精度值FimX是指所述机械手臂2所要到达目标位置的到达程度。步骤S31，根据输入的机械手臂2的初始位置参数值和目标位置参数值，利用迭代 法计算每次迭代后每个关节的末节点所到达的位置。步骤S32，根据上述计算的每次迭代中每个关节的末节点所到达的位置，计算每次 迭代中每个关节的末节点所到达位置与目标位置之间的距离，并得到其中最小距离的每个 关节所到达的位置及对应该位置的每个关节的末节点的坐标值。(具体步骤将在图4中详 细描述)步骤S33，每个关节上的马达3驱动该关节移动到步骤S32得到的与目标位置距离 最小的位置，从而使得该机械手臂2的每个关节以最快速度到达最优位置。如图4所示，是图3中步骤S31和步骤S32的细化流程图。步骤S320，利用所述机械手臂2的每个关节的末节点初始位置坐标值，计算得 到该机械手臂2的所有关节末节点从初始位置坐标值到达目标位置坐标值的距离平方和
其中η表示关节个数，dn表示第η个关节与该关节的目标位置
之间的距离，例如dl表示图2中第一关节的末节点B从初始位置(X1B，Y1B, ZIb)移动到 目标位置(X2b，Y2b，Z2b)所移动的距离，则dl」u^'-ZV2+(IV-KV2+GV-ZV2。其中 OV，Yb’，V )为该第一关节末节点B到达距离目标位置最近点的坐标值，该坐标值为未
知参数。该坐标值可由公式UOJiZJ"
算得出关于角度α的表达式，该角度α表示该第一关节所旋转的角度。同理，每个关节 的末节点坐标都可表示为关于该关节旋转角度的表达式，将该计算得到的坐标值代入函数
Mn^dn" I使用拟牛顿迭代法，取不同旋转角度值作为迭代步长，得到距离目标位 f (x)=，置距离平方和f(x)的最小值及取得该f(x)最小值的旋转角度值，所述f(x)最小即距离目 标位置的距离最小。步骤S321，判断上述计算得到的f(x)是否小于精度值FunX。若f(x)不小于FunX， 则进入步骤S322。若f (χ)小于FunX，则结束流程。步骤S322，利用数学法则如拟牛顿迭代法计算f(x)的下降方向。所述下降方向 指使f(x)的值变小的方向，即使所述机械手臂2的所有关节距离目标位置的距离变小的方 向。步骤S323，判断是否存在所述下降方向。若存在所述下降方向，则进入步骤S324。 若不存在下降方向，则结束流程。步骤S324，计算所有关节沿下降方向移动迭代步长后到达目标位置的距离平方和 f(x+l)。其中，所述f(x+l)与步骤S320中f(x)的计算方法完全相同，仅仅使用的参数不 同，可参考步骤S320完成计算。步骤S325，判断上述计算得到的f(x+l)是否小于f(x)。若f (χ+l)小于f (χ)，则 返回步骤S322 ；若f(x+l)不小于f(x)，则返回步骤S324。最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参 照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明 的技术方案进行修改或等同替换，而不脱离本发明技术方案的精神和范围。
权利要求
一种机械手臂反向运动方法，其特征在于，该方法包括a)输入机械手臂初始位置的参数值和目标位置参数值，所述机械手臂的初始位置参数值包括初始位置上每个关节的根节点和末节点的坐标值，所述目标位置参数值包括目标位置上每个关节的末节点的坐标值；b)根据输入的初始位置参数值和目标位置参数值，利用迭代法计算每次迭代后每个关节的末节点所到达的位置；c)根据上述计算的每次迭代中每个关节的末节点所到达的位置，计算每次迭代中每个关节的末节点所到达的位置与目标位置之间的距离，并得到其中最小距离的每个关节的位置及对应该位置的每个关节末节点的坐标值；及d)每个关节上的马达驱动该关节移动到达上述与目标位置距离最小的位置。
2.如权利要求1所述的机械手臂反向运动方法，其特征在于，所述步骤a)包括输入 用户设置的精度值。
3.如权利要求2所述的机械手臂反向运动方法，其特征在于，所述步骤c)包括利用所述机械手臂的每个关节的末节点初始位置坐标值，计算得到该机械手臂的所有 关节末节点从初始位置坐标值到达目标位置坐标值的距离平方和f (x)；当f(X)小于用户设置的精度值时，该f(x)即为所述每个关节的末节点距离目标位置 最小距离平方和；当f(X)不小于所述精度值时，计算f(x)的下降方向，所述下降方向指使f(x)的值变 小的方向；当不存在所述下降方向时，该f(X)即为所述每个关节的末节点距离目标位置最小距 离平方和；当存在所述下降方向时，计算所有关节沿下降方向移动迭代步长后到达目标位置的距 离平方和f(x+l)；当f(x+l)不小于f(x)时，返回步骤计算所有关节沿下降方向移动迭代步长后到达目 标位置的距离平方和f(X+l)；及当f(X+l)小于f(x)时，返回计算f(x)的下降方向的步骤。
4.如权利要求3所述的机械手臂反向运动方法，其特征在于，所述迭代步长是指每个 关节的旋转角度。
全文摘要
一种机械手臂反向运动方法，该方法包括输入机械手臂初始位置的参数值和目标位置的参数值；根据输入的初始位置参数值和目标位置参数值，利用迭代法计算每次迭代后每个关节的末节点所到达的位置；根据上述计算的每次迭代中每个关节的末节点所到达的位置，计算每次迭代所到达位置与目标位置之间的距离，并得到其中最小距离的每个关节的位置及对应该位置的每个关节关节点的坐标值；及每个关节上的马达驱动该关节移动到达与目标位置距离最小的位置。
文档编号B25J9/00GK101870104SQ20091030184
公开日2010年10月27日 申请日期2009年4月25日 优先权日2009年4月25日
发明者吴新元, 张旨光 申请人:鸿富锦精密工业(深圳)有限公司;鸿海精密工业股份有限公司