一种6R工业机器人逆运动学求解方法与流程

本发明涉及一种工业机器人逆运动学领域，特别涉及一种6r工业机器人逆运动学求解方法。

背景技术：

目前，工业机器人运动学是机器人学重要研究方向之一，它对机器人控制、机器人动力学和轨迹规划有很大影响，是研究机器人动力学、轨迹规划基础。它从几何或机构角度描述和研究了机器。人运动特性，而不考虑引起这些运动的力或力矩的作用，分为正运动学与逆运动学。正运动学是对给定的机器人，已知连杆几何参数和关节变量，求机器人末端执行器相对于参考坐标系的位置和姿态。逆运动学是已知连杆几何参数和机器人末端执行器相对于参考坐标系的位置和姿态，来求解能达到预期位姿的机器人关节变量。

目前常用的机器人运动学方法主要是解析法和数值法，解析法比如矩阵法，它需要在每个关节上建立相应的坐标系，采用坐标变换的方式建立正运动学模型，在逆运动学求解上，通常采用反变换矩阵方法，其求解过程复杂，结果相对比较繁琐，效率比较低。数值解法直接求解约束方程组，可以通过迭代运算求得任何机构的实数解，但通常不能得到全部解，一般而言，初值的选取及搜索算法对收敛性和精度影响较大。几何逆解法已经有部分学者研究，针对某种结构，有一定的局限性。

技术实现要素：

本发明为解决公知技术中存在的技术问题而提供一种高效的6r工业机器人逆运动学求解方法。

本发明为解决公知技术中存在的技术问题所采取的技术方案是：一种6r工业机器人逆运动学求解方法，该方法包括如下步骤：

步骤一：建立6r机器人的坐标系、几何简化模型及d-h表；

步骤二：根据几何简化模型，建立第一关节的关节角与末端连杆坐标系原点的空间坐标间的函数关系式；

步骤三：在已确定第一关节的关节角条件下，将第二连杆及第三连杆的空间位置关系简化为三角形，建立第二、三关节的关节角的函数关系式；

步骤四：在已确定第一至第三关节的关节角条件下，将第四连杆坐标系、第五连杆坐标系及第六连杆坐标系的z轴方向向量，以第五关节旋转轴为中心轴建立空间几何模型，由空间几何模型，先建立第五关节的关节角函数关系式；再建立第四、第六关节的关节角的函数关系式。

进一步地，步骤一中，由pieper准则将第四至第六关节旋转轴的空间交点作为第四至第六关节坐标系共同的坐标原点，建立6r机器人的几何简化模型。

进一步地，步骤二中，建立第一关节的关节角与末端连杆的空间坐标之间的函数关系式的方法包括如下分步骤：

步骤1-1，设置中间变量θ1_pre、θ1_var为如下所示：

式中，d3为第三连杆偏距；px、py,、pz对应为末端连杆的坐标系原点的空间坐标；

步骤1-2，设θ1为第一关节的关节角；则由中间变量θ1_pre、θ1_var，得到第一关节的关节角的可能解如下：

步骤1-3，将当前θ1的两个解与前一个时刻θ1的值相减，选择差的绝对值比较小所对应的解作为第一关节的关节角。

进一步地，步骤三包括如下分步骤：

步骤3-1，将第二连杆和第三连杆的空间关系简化为三角形，设三角形的第一个边长为l1，第二个边长为l2，第三个边长为l3；设l2与l1之间的夹角为q1，l2与l3之间的夹角为q2，l1与水平线之间的夹角为q3；其中l2等于第二连杆的长度；l3等于由第三连杆的长度和第四关节的连杆偏距所构成的直角三角形的斜边的长度；l1等于末端连杆坐标系原点与第二连杆坐标系原点间的空间直线距离；根据空间几何关系，得到如下q1、q2、q3与l1、l2、l3之间的函数关系式：

式中，d3为第三关节的连杆偏距；px、py,、pz对应为末端连杆的坐标系原点的空间坐标；

步骤3-2，设θ2为第二关节的关节角；设θ3为第三关节的关节角；得到如下θ2、θ3与q1、q2、q3之间的函数关系式：

若θ2＝-(q1+q3)，则有：

若θ2＝q1-q3，则有：

式中，d4为第四关节的连杆偏距；a3为第三连杆的长度；

步骤3-3，计算第二关节的关节角与第一关节的关节角的差的绝对值；θ2选择两者差的绝对值比较小所对应的值。

进一步地，步骤四包括如下分步骤：

步骤4-1，设o4z、o5z、o6z分别对应为第四、第五、第六连杆坐标系的z轴方向向量；将o4z、o5z、o6z以第五关节的关节轴为中心轴建立空间几何模型；

步骤4-2，设第五关节的关节角为θ5；由o4z与o6z的点积方程得到如下θ5与o4z、o6z间的函数关系式：

θ5＝arccos(o4z·o6z)或θ5＝-arccos(o4z·o6z)；

将当前θ5的两个解与前一个时刻θ5的值相减，选择差的绝对值比较小所对应的解作为第五关节的关节角；

步骤4-3，设o6z′为期望的末端连杆坐标系的z轴方向向量；将o6z绕第四关节旋转轴旋转形成第一圆锥，设o6z"为o6z绕第五关节旋转轴旋转并位于第一圆锥上的方向向量；设r1、r2分别对应为o6z"、o6z′在圆锥底面的投影；设第四关节的关节角为θ4；得到如下θ4与r1、r2之间的函数关系式：

或

由r1与r2的叉积再与o4z点积的值的正负确定θ4的正负；

步骤4-5，在确定第一至第五关节的关节角后，使第六关节旋转至满足期望的末端连杆坐标系x方向，设o6x为末端连杆坐标系的x轴方向向量，o6x"为期望的末端连杆坐标系的x轴方向向量；由o6x与o6x"的点积方程得到如下θ6与o6x、o6x"间的函数关系式：

cos(θ6)＝o6x".o6x'；

θ6＝arccos(o6x".o6x')或θ6＝-arccos(o6x".o6x')；

由o6x和o6x"的叉积再与o6z点积的值的正负确定θ6的正负。

本发明具有的优点和积极效果是：本发明对关节机器人进行简化，参考craigjj提出的机器人坐标系建立规则，建立该机器人的坐标系，对于常见的三种6r机器人建立几何简化模型，与现有技术相比，本发明提出机器人逆解方法解决了传统矩阵法计算复杂问题，同时提高了效率，有效的提高了姿态精度，对于机器你人轨迹规划和控制评估具有重要影响。

附图说明

图1是本发明的一种6r机器人构型1的结构简图与几何简图的对照图；

图2是本发明的一种6r机器人构型2的结构简图与几何简图的对照图；

图3是本发明的一种6r机器人构型3的结构简图与几何简图的对照图；

图4是本发明的一种6r机器人的第二连杆和第三连杆空间位置在基坐标系的投影示意图；

图5是本发明的一种6r机器人的第二连杆和第三连杆的空间简化三角形示意图；

图6是本发明的一种6r机器人后第四至第六关节旋转变换几何示意图；

图7是本发明的一种6r机器人未旋转第六关节角前末端连杆坐标系示意图。

具体实施方式

为能进一步了解本发明的发明内容、特点及功效，兹列举以下实施例，并配合附图详细说明如下：

请参见图1至图7，一种6r工业机器人逆运动学求解方法，该方法包括如下步骤：

步骤一：建立6r机器人的坐标系、几何简化模型及d-h表；

步骤二：根据几何简化模型，建立第一关节的关节角与末端连杆坐标系原点的空间坐标间的函数关系式；

步骤三：在已确定第一关节的关节角条件下，将第二连杆及第三连杆的空间位置关系简化为三角形，建立第二关节的关节角、第三关节的关节角的函数关系式；

步骤四：在已确定第一关节的关节角、第二关节的关节角、第三关节的关节角的条件下，将第四连杆坐标系、第五连杆坐标系及第六连杆坐标系的z轴方向向量，以第五关节旋转轴为中心轴建立空间几何模型，由空间几何模型，先建立第五关节的关节角函数关系式；再建立第四关节的关节角、第六关节的关节角的函数关系式。

优选地，步骤一中，可由pieper准则将第四关节旋转轴、第五关节旋转轴及第六关节旋转轴的空间交点作为第四关节坐标系、第五关节坐标系及第六关节坐标系共同的坐标原点，建立6r机器人的几何简化模型。

优选地，步骤二中，可建立第一关节的关节角与末端连杆的空间坐标之间的函数关系式的方法可包括如下分步骤：

步骤1-1，可设置中间变量θ1_pre、θ1_var为如下所示：

式中，d3为第三连杆偏距；px、py,、pz对应为末端连杆的坐标系原点的空间坐标；

步骤1-2，可设θ1为第一关节的关节角；则由中间变量θ1_pre、θ1_var，得到第一关节的关节角的可能解如下：

步骤1-3，将当前θ1的两个解与前一个时刻θ1的值相减，选择差的绝对值比较小所对应的解作为第一关节的关节角。

优选地，步骤三可包括如下分步骤：

步骤3-1，将第二连杆和第三连杆的空间关系简化为三角形，设三角形的第一个边长为l1，第二个边长为l2，第三个边长为l3。三角形第一边的几何意义是末端连杆坐标系原点与第二连杆坐标系原点的空间直线距离，第二边相当于第二连杆长度，第三边相当于第三连杆长度。设l2与l1之间的夹角为q1，l2与l3之间的夹角为q2，l1与水平线之间的夹角为q3；其中l2等于第二连杆的长度；l3等于由第三连杆的长度和第四关节的连杆偏距所构成的直角三角形的斜边的长度；l1等于末端连杆坐标系原点与第二连杆坐标系原点间的空间直线距离；根据空间几何关系，得到如下q1、q2、q3与l1、l2、l3之间的函数关系式：

式中，d3为第三关节的连杆偏距；px、py,、pz对应为末端连杆的坐标系原点的空间坐标；

步骤3-2，可设θ2为第二关节的关节角；设θ3为第三关节的关节角；得到如下θ2、θ3与q1、q2、q3之间的函数关系式：

若θ2＝-(q1+q3)，则有：

若θ2＝q1-q3，则有：

式中，d4为第四关节的连杆偏距；a3为第三连杆的长度；

步骤3-3，计算第二关节的关节角与第一关节的关节角的差的绝对值；θ2选择两者差的绝对值比较小所对应的值。

优选地，步骤四可包括如下分步骤：

步骤4-1，设o4z、o5z、o6z分别对应为第四、第五、第六连杆坐标系的z轴方向向量；将o4z、o5z、o6z以第五关节的关节轴为中心轴建立空间几何模型。

步骤4-2，设第五关节的关节角为θ5；由o4z与o6z的点积方程得到如下θ5与o4z、o6z间的函数关系式：

θ5＝arccos(o4z·o6z)或θ5＝-arccos(o4z·o6z)；

将当前θ5的两个解与前一个时刻θ5的值相减，选择差的绝对值比较小所对应的解作为第五关节的关节角。

步骤4-3，可设o6z′为期望的末端连杆坐标系的z轴方向向量；可将o6z绕第四关节旋转轴旋转形成第一圆锥，可设o6z"为o6z绕第五关节旋转轴旋转并位于第一圆锥上的方向向量；可设r1、r2分别对应为o6z"、o6z′在圆锥底面的投影；设第四关节的关节角为θ4；得到如下θ4与r1、r2之间的函数关系式：

或

可由r1与r2的叉积再与o4z点积的值的正负确定θ4的正负；第四关节的关节角θ4正负性与r1和r2的叉积再与连杆坐标系o4z点积的正负有关的。当r1和r2的叉积再与连杆坐标系o4z点积的值为负值时，第四关节的关节角θ4取正值；当r1和r2的叉积再与连杆坐标系o4z点积的值为正值时，第四关节的关节角θ4取负值。从而可以确定第四关节的关节角的唯一值。

步骤4-5，在确定第一至第五关节的关节角后，可使第六关节旋转至满足期望的末端连杆坐标系x方向，可设o6x为末端连杆坐标系的x轴方向向量，o6x"为期望的末端连杆坐标系的x轴方向向量；由o6x与o6x"的点积方程得到如下θ6与o6x、o6x"间的函数关系式：

cos(θ6)＝o6x".o6x'；

θ6＝arccos(o6x".o6x')或θ6＝-arccos(o6x".o6x')；

可由o6x和o6x"的叉积再与o6z点积的值的正负确定θ6的正负。当o6x"和o6x的叉积再与末端连杆坐标系o6z点积的值为正值时，第六关节的关节角θ4取正值；当o6x"和o6x的叉积再与末端连杆坐标系o6z点积的值为负值时，第六关节的关节角θ4取负值；从而可以确定第六关节的关节角。

下面通过本发明的一个优选实施例来进一步说明本发明的工作流程及工作原理：

本发明的优选实施例以常见的球形腕6r机器人为对象，针对3种类型关节机器人进行简化，3种类型关节机器人分别简称构型1、构型2及构型3。

根据pieper原则，参考craigjj提出的机器人坐标系建立规则，建立该机器人的坐标系，对于常见的三种6r机器人建立几何简化模型，这三种构型的区别在第一个关节和连杆上。同时建立相对应的d-h表，它作用是为后三个关节的关节角的计算做基础，即计算连杆坐标系的方向向量以便进行矢量运算。以已知的连杆参数为基准，将关节机器人简化为六轴机械连杆，根据pieper准则可将后三轴的空间交点作为后三轴共同的坐标原点，从而可将机器人几何连杆简化。

首先根据6r机器人的构型，建立6r机器人的坐标系、几何简化模型及d-h表。

其中，几何简化模型请参见图1至图3。

d-h表建立如下表一至表三所示。

表一：6r机器人构型1的d-h表

表二：6r机器人构型2的d-h表

表三6r机器人构型3的d-h表

根据已建立6r机器人的坐标系、几何简化模型及d-h表，根据几何关系，先求出第一关节的关节角；其方法如下：

首先建立机器人的dh参数表如表1。根据图4，第一关节的关节角与已知参数的函数关系推导如下：

式中θ1_pre，θ1_var均为计算第一关节的关节角的中间变量；d3为第三关节的连杆偏距；px,py,pz为机器人末端连杆的附带坐标系的原点的空间坐标。

第一关节的关节角有两种可能解如下：

式中θ1为计算关节1的关节角。

需要计算两种解与前一个时刻关节角θ1差的绝对值，选择绝对值比较小对应的关节角，作为关节1的关节角。

在第一关节的关节角的情况下，请参见图5，将机器人大臂和小臂简化为三角形，通过几何关系，求得第二关节的关节角和第三关节的关节角。

图5中的参数l2为机器人大臂的长度，数值大小上等于第二连杆的连杆长度参数a2，参数l3由连杆参数a3和d4通过几何关系来取得，几何意义是末端连杆坐标系原点与第二连杆坐标系的原点空间直线距离。l1与l2，l3组成三角形，其中l2与l1之间的夹角为q1，l2与l3之间的夹角为q2，l1与水平线之间的夹角为q3。

对于机器人构型1的角度q1和q2分别为

机器人构型3的角度q3为

关节2的角度有两种情况:

若θ2＝-(q1+q3)，则

若θ2＝q1-q3，则

根据前三个关节，通过关节的空间几何关系，首先求出关节5，然后求出关节角4和6。具体方法如下：

请参考图6，以关节轴4为中心轴，以o6z'为母线。o4z,o5z,o6z分别是在前三个关节角条件下的第四连杆坐标系、第五连杆坐标系和第六连杆坐标系的z轴方向向量。o6z'为期望的末端连杆坐标系的z轴方向向量，o6z"为将o6z通过绕第五关节轴得到方向向量，其与o6z在一个圆锥上。r1、r2分别为o6z"与o6z在圆锥底面的投影。

第五关节的关节角可以由o4z与o6z点积得到即：

cos(θ5)＝o4z·o6z(10)

θ6＝arccos(o6x".o6x')或θ6＝-arccos(o6x".o6x')

将当前θ5的两个解与前一个时刻θ5的值相减，选择差的绝对值比较小所对应的解作为第五关节的关节角。

同理，通过图5的几何关系来求解第四关节的关节角，第四关节的关节角可以表示为

或

第四关节的关节角θ4正负性与r1和r2的叉积再与连杆坐标系o4z点积的正负有关的。当r1和r2的叉积再与连杆坐标系o4z点积的值为负值时，第四关节的关节角θ4取正值；当r1和r2的叉积再与连杆坐标系o4z点积的值为正值时，第四关节的关节角θ4取负值。从而可以确定第四关节的关节角的唯一值。

在前面第一至第五关节的关节角确定以后，只能满足期望的末端连杆坐标系z方向，故需要把末端连杆坐标系其它的两个方向达到期望位置，即末端连杆坐标系的x轴方向和y轴方向达到期望位置，这需要关节6的旋转。如图7所示。

图7中用实线表示的坐标系是末端连杆期望的位姿，用虚线表示的坐标系为第六关节未旋转之前，即在前5个关节的关节角已确定条件下的末端连杆附带的坐标系。第六关节的关节角可以由o6x与o6x"点积得到即：

cos(θ6)＝o6x".o6x'(12)

θ6＝arccos(o6x".o6x')或θ6＝-arccos(o6x".o6x')

第六关节的关节角θ6正负性与o6x和o6x"的叉积再与末端连杆坐标系o6z点积的正负有关的。当o6x"和o6x的叉积再与末端连杆坐标系o6z点积的值为正值时，第六关节的关节角θ4取正值；当o6x"和o6x的叉积再与末端连杆坐标系o6z点积的值为负值时，第六关节的关节角θ4取负值；从而可以确定第六关节的关节角。

以上所述的实施例仅用于说明本发明的技术思想及特点，其目的在于使本领域内的技术人员能够理解本发明的内容并据以实施，不能仅以本实施例来限定本发明的专利范围，即凡本发明所揭示的精神所作的同等变化或修饰，仍落在本发明的专利范围内。