CN104866722A - 一种七轴工业机械臂的逆动学求解方法 - Google Patents

Abstract

本发明实施例提供了一种七轴工业机械臂的逆动学求解方法，根据每两个离散点得到第一关节角的变化趋势得到离散点集合中每一点对应的第一关节角的值；采用每个离散点的末端位姿矩阵，获取第五或第六姿态的坐标原点，再求出第三姿态的坐标原点，求出第二关节角；根据第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系求解第四关节角；由关于第一关节角、第二关节角、第四关节角的方程以及第三关节角的两个等式，求出第三关节角；获取第四姿态的坐标原点坐标，求出第六关节角的特征角，根据第六关节角的特征角求解第六关节角；根据第六关节角的求解结果，分情况求解第五关节角和第七关节角；具有很好的通用性。

Description

一种七轴工业机械臂的逆动学求解方法

技术领域

本发明涉及涉及一种七轴工业机械臂的逆动学求解方法，尤其涉及七轴工业机械臂的建模方法。

背景技术

在现有技术中七轴工业机械臂的逆动学求解方法有如下两种：

现有技术的方法如二次规划法，几何分析法以及位姿分解方法等，

现有技术的方法(一):二次规划法在祖迪，吴镇炜的《一种冗余机器人逆运动学求解得有效方法》中描述的较为详细。该方法重点在首先利用雅可比矩阵以及梯度投影法求出一组关节角，在二次计算时固定其中一个角度来求解剩余关节角；

对于方法(二)所述：几何分析法在崔泽，韩增军的《基于自运动的仿人七自由度机械臂逆解算法》中描述的较为详细。该方法重点在自运动构型的基础上，以0坐标系为其他坐标系的基系，根据同一向量在不同坐标系之间的投影特性与关节角之间存在着的对应关系，实现对七自由度机械臂逆运动学的求解；

对于方法(三)所述：位姿分解方法在潘博，付宜利的《面向冗余机器人实时控制的逆运动学求解有效方法》所述比较详细。该方法针对具有球形手腕的7自由度机械臂结构特点，采用位姿分离的方法，将7自由度问题降为4自由度位置冗余问题以及3自由度姿态问题，在此基础之上再利用梯度投影法进行逆运动学的求解。

在现有的三种方案中，上述现有技术中的的缺点为：

方法(一)所述方法中梯度投影法中，自运动放大系数k选取为常数，第一需要经过反复的计算机仿真通常才能确定一个k值，同时固定k值，缺少机器人运动优化能力，常导致机器人关节角震荡。同时二次计算虽然精度高但是运算量大，实时性差；

方法(二)虽然利用自运动轨迹和几何方法可以求解逆运动学，但是该方法只适用于两杆之间存在轴线平行于连杆的转动副的情况，即存在横滚转动副的七自由度机器人才能够如此简化，不具有很强的通用性；

方法(三)利用位姿分解能简化逆运动学的求解，同方法二一样也限制于机械臂的构型，不具有很好的通用性。

发明内容

本发明的实施例提供了一种七轴工业机械臂的逆动学求解方法，本发明提供了如下方案，包括：

步骤1、将虚拟机械臂末端执行器任务设定的起始点和终止点中间对应的位置离散化，获取离散点；

步骤2、建立第一关节角和第七姿态的坐标原点的平面方程，根据每两个离散点得到第一关节角的变化趋势，根据所述变化趋势得到离散点集合中每一点对应的第一关节角的值；

其中，第一平面坐标系为基座底部坐标点、第零姿态的坐标原点以及第一或第二姿态的坐标原点构成的平面；

步骤3、采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵T，根据变换矩阵获取第五或第六姿态的坐标原点，再确定位姿矩阵，根据位姿矩阵求出第三姿态的坐标原点；根据第五或第六姿态的坐标原点和第三姿态的坐标原点的距离方程式，求出第二关节角；

步骤4、建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系，根据所述条件关系求解第四关节角；其中，

所述第四关节角的第一特征角为以第三姿态的坐标原点为顶点，与第四姿态的坐标原点及第五或第六姿态坐标原点形成的夹角；

所述第四关节角的第二特征角以第三姿态的坐标原点为顶点，与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的夹角；

步骤5、由正运动学方程的左右两边的第四列对应相等得到的方程得到关于第一关节角、第二关节角、第四关节角以及第三关节角的两个等式，从而求出第三关节角；

步骤6、确定位姿矩阵，根据位姿矩阵得到第四姿态的坐标原点坐标，再求出第四姿态的坐标原点和第七姿态的坐标原点之间的空间距离，再由第四姿态的坐标原点、第五或第六姿态的坐标原点和第七姿态的坐标原点构成的三角形中，求出第六关节角的特征角，根据第六关节角的特征角求解第六关节角；其中，第六关节角的特征角为以第五或第六姿态的坐标原点为顶点，与第四姿态的坐标原点、第七姿态的坐标原点形成的夹角；

步骤7、根据第六关节角的求解结果，结合末端坐标的关系，分情况求解第五关节角和第七关节角。

根据本发明的上述方法，在规划第一关节角之前，包括：

以虚拟机械臂末端执行器的以基座的底部点O为基坐标点，再另外依次确定七个位姿状态对应的空间坐标系的坐标原点，其中第一姿态和第二姿态公用同一个坐标原点为第一或第二姿态的坐标原点，第五姿态和第六姿态公用同一个坐标原点为第五或第六姿态的坐标原点；

根据本发明的上述方法，所述建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系，包括：第四关节角的第一特征角与第四关节轴的变换关系：当第四关节角的第一特征角变小时，该变化量是由于第四关节轴向上俯仰造成，同理当第四关节角的第一特征角变大时，该变化量是由于第四关节轴向下俯仰造成，根据所述变换关系建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系。

根据本发明的上述方法，包括：在建立第一关节角和第七姿态的坐标原点的平面方程之前，

旋转第一关节角，将第七姿态的坐标原点旋转至第一平面坐标系内，使第一关节角和第七姿态的坐标原点处于同一平面。

根据本发明的上述方法，包括：

所述第四关节角的第一特征角通过以第三姿态的坐标原点为顶点，与第四姿态的坐标原点及第五或第六姿态坐标原点形成的直角三角形中的三角函数求解；

所述第四关节角的第二特征角通过以以第三姿态的坐标原点为顶点，与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的三角形的三角函数求解。

根据本发明的上述方法，根据第六关节角的特征角求解第六关节角，包括：第六关节角对应为两个解，具体为第六关节角的特征角的补角的正负值。

根据本发明的上述方法，根据第六关节角的求解结果，结合末端坐标的关系，分情况求解第五关节角和第七关节角，包括：

当第六关节角不为为零时，由末端坐标的关系求出第五姿态角，再由末端姿态的关系求出第七姿态角；

当第六关节角为零时，机械臂处于奇异位置，不能唯一确定第五关节角和第七关节角，令第五关节角等于零，则可求出第七关节角，得到第五关节角和第七关节角的一组特解。

由上述本发明的实施例提供的技术方案可以看出，本发明实施例根据每两个离散点得到第一关节角的变化趋势得到离散点集合中每一点对应的第一关节角的值；采用每个离散点的末端位姿矩阵，获取第五或第六姿态的坐标原点，再求出第三姿态的坐标原点，求出第二关节角；根据第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系求解第四关节角；由关于第一关节角、第二关节角、第四关节角的方程以及第三关节角的两个等式，求出第三关节角；获取第四姿态的坐标原点坐标，求出第六关节角的特征角，根据第六关节角的特征角求解第六关节角；根据第六关节角的求解结果，分情况求解第五关节角和第七关节角；具有很好的通用性。计算精度较高，且实时性好，并且具有很好的通用性。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1本发明实施例一提供的一种七轴工业机械臂的原理图；

图2为本发明实施例一提供的一种七轴工业机械臂的逆动学求解方法

的处理流程图。

具体实施方式

为便于对本发明实施例的理解，下面将结合附图以几个具体实施例为例做进一步的解释说明，且各个实施例并不构成对本发明实施例的限定。

实施例一

以虚拟机械臂末端执行器的基座的底部点O为基坐标点，再另外依次确定七个位姿状态对应的空间坐标系的坐标原点，其中第一姿态和第二姿态公用同一个坐标原点为第一或第二姿态的坐标原点，第五姿态和第六姿态公用同一个坐标原点为第五或第六姿态的坐标原点；

由附图1可以得到一个D-H参数表：

除去第一个和最后一个连杆外，每个连杆两端的轴线各有一条法线，分别为前，后相邻连杆的公共法线。这两法线间的距离即为d_i。把连杆长度称为a_i，α_i为连杆扭角，θ_i为两连杆夹角，即表示为第i个关节角，本实施例中，θ₁表示第一关节角、θ₂表示第二关节角、θ₃表示第三关节角、θ₄表示第四关节角、θ₅表示第五关节角、θ₆表示第六关节角、θ₇表示第七关节角。

通过上述D-H参数可以得到第i坐标系相对于第i-1坐标系的转换矩阵

$T_i^{i-1}=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_i& -\sin {\mathrm{\θ}}_i\cos {\mathrm{\α}}_i& \sin {\mathrm{\θ}}_i\sin {\mathrm{\α}}_i& a_i\cos {\mathrm{\θ}}_i\\ \sin {\mathrm{\θ}}_i& \cos {\mathrm{\θ}}_i\cos {\mathrm{\α}}_i& -\cos {\mathrm{\θ}}_i\sin {\mathrm{\α}}_i& a_i\sin {\mathrm{\θ}}_i\\ 0& \sin {\mathrm{\α}}_i& \cos {\mathrm{\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]$

则第七坐标系相对于第0坐标系的转换矩阵如下，

由于是冗余机械臂，七个关节角存在着耦合关系，因此利用几何方法求逆，必须先规划出一个关节角的值。考虑到本发明基于的机械臂的机械构型，关节1和关节3都为旋转轴，这里将第一轴虚拟为冗余轴，首先规划出关节角1在每个离散点的值。

如附图1所示，以基座的底部点O为基坐标点，点O₀,A,B,B₁,C,D依次对应0,1(2),3,4,5(6),7坐标系的原点位置，0,1(2),3,4,5(6),7坐标系是每个转动关节对应的每个虚拟坐标系，每个坐标系对应一个转动关节，第0坐标系对应基座关节，1～7坐标系分别对应1～7转动关节,点O₀,A,B,B₁,C,D依次是每个转动关节对应的虚拟坐标系的原点，其中,

第零坐标系的坐标原点对应O₀,为第零姿态的坐标原点O₀；

第一和第二坐标系的坐标原点重叠对应A,为第一或第二姿态的坐标原点A；

第三坐标系的坐标原点对应B,为第三姿态的坐标原点B；

第四坐标系的坐标原点对应B₁,为第四姿态的坐标原点B₁；

第五和第六坐标系的坐标原点重叠对应C,为第五或第六姿态的坐标原点C；

第七坐标系的坐标原点对应D，为第七姿态的坐标原点D。

初始末端位姿矩阵T为：

$T=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$

初始末端位姿矩阵T是末端坐标系在基坐标系内的转换矩阵，其中，(n_x，n_y，n_z)表示虚拟机械臂末端坐标系的x轴在基坐标系下的坐标表示，(o_x，o_y，o_z)表示虚拟机械臂末端坐标系的y轴在基坐标系下的坐标表示，

(a_x，a_y，a_z)表示虚拟机械臂末端坐标系的z轴在基坐标系下的坐标表示，且矢量(n_x，n_y，n_z)，(o_x，o_y，o_z)，(a_x，a_y，a_z)均已单位化，(p_x，p_y，p_z)表示虚拟机械臂末端坐标系的原点在基坐标系下坐标；则第七姿态的坐标原点D的坐标即为(p_x p_y p_z)′。

该实施例提供了一种七轴工业机械臂的逆动学求解方法的处理流程如图2所示，包括如下的处理步骤：

步骤1、将虚拟机械臂末端执行器任务设定的起始点和终止点中间对应的位置离散化，获取离散点；

步骤2、建立第一关节角和第七姿态的坐标原点的平面方程，根据每两个离散点得到第一关节角的变化趋势，根据所述变化趋势得到离散点集合中每一点对应的第一关节角的值；

具体地，旋转第一关节角，将第七姿态的坐标原点旋转至第一平面坐标系内，使第一关节角和第七姿态的坐标原点处于同一平面，建立第一关节角和第七姿态的坐标原点的平面方程，根据每两个离散点得到第一关节角的变化趋势，根据所述变化趋势得到离散点集合中每一点对应的第一关节角的值；

其中，第一平面坐标系为基座底部坐标点、第零姿态的坐标原点以及第一或第二姿态的坐标原点构成的平面。

如附图1所述，将第七姿态的坐标原点D旋转至第一平面AO₀O内，使得第一关节角和第七姿态的坐标原点D处于同一个平面，再利用平面方程得到每个离散点与对应的第一关节角θ₁的线性关系，

具体做法是旋转第一关节角，使得第七姿态的坐标原点D在平面AO₀O内，而平面AO₀O的方程为：

-tanq₁x+y＝0

再将第七姿态的坐标原点D代入上述方程，即可得出q₁＝atan2(p_y,p_x)

根据每两个离散点得到第一关节角θ₁的变化趋势，根据所述变化趋势得到离散点集合中每一点对应的第一关节角θ₁的值，即利用该变化趋势得到第一关节角θ₁从起始点到末端点中每个离散点对应的θ₁的值；具体地，如下：序号0表示初始点，1表示第一个离散点，2表示第二个离散点；q₁₀表示初始点(即第0个点)处使得末端坐标系原点在平面AO₀O内所需的第一关节角的值，可以认为理想值，而θ₁₀表示初始点处第一关节角的实际值，q₁₁表示第一个离散点处第一关节角的理想值，因此第一个离散点处第一关节角的实际值θ₁₁就应该由公式θ₁₁＝θ₁₀+q₁₁-q₁₀得出，后面的迭代即可；

步骤3、采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵T，根据变换矩阵获取第五或第六姿态的坐标原点，再确定位姿矩阵，根据位姿矩阵求出第三姿态的坐标原点；根据第五或第六姿态的坐标原点和第三姿态的坐标原点的距离方程式，求出第二关节角；

具体地，由正运动学得到位姿矩阵等式两边运算得到变换矩阵：其中，采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵T，根据变换矩阵获取第五或第六姿态的坐标原点，再通过正运动学得到位姿矩阵采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵，结合位姿矩阵求出第三姿态的坐标原点；根据空间中两点的距离公式建立第五或第六姿态的坐标原点和第三姿态的坐标原点的距离方程式，求出第二关节角。

由正运动学的知识可以得到以下位姿矩阵，等式两边进行一定的运算得到：

$T_6^0{=}_1^0{T}_2^1{T}_3^2{T}_4^3{T}_6^{5}T=T{\left(T_7^6\right)}^{-1}$

采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵，每个离散点的末端位姿矩阵的一到三行第4列可得出第五或第六姿态的坐标原点C点坐标(x_cy_cz_c)，再通过正运动学采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵求出第三姿态的坐标原点B的坐标(x_by_bz_b)，利用空间中两点的距离公式可以得出两个方程式，并求出第二关节角θ₂；

具体地，公式即为由于以每个离散点的末端位姿矩阵作为初始末端位姿矩阵，每个离散点的初始末端位姿矩阵T已知，且转换矩阵的最后一列已知，则可以求出的最后一列，其前三项即为C点坐标；再由公式可计算出出的最后一列只与θ₁,θ₂有关，而θ₁已知，因此最后一列的前三项(B点坐标)只与θ₂有关，因此根据空间中两点间距离公式可以确定|BC|只与θ₂有关。另一方面，在直角三角形Rt△BB₁C中，直角边|BB₁|,|B₁C|为杆长已知不变，因此可以求出斜边|BC|，故可以求出第二关节角θ₂。

步骤4、建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系，根据所述条件关系求解第四关节角；其中，

第四关节角的第一特征角为以第三姿态的坐标原点为顶点，与第四姿态的坐标原点及第五或第六姿态坐标原点形成的夹角；

第四关节角的第二特征角以第三姿态的坐标原点为顶点，与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的夹角。

其中，建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系，包括：第四关节角的第一特征角与第四关节轴的变换关系：当第四关节角的第一特征角变小时，该变化量是由于第四关节轴向上俯仰造成，同理当第四关节角的第一特征角变大时，该变化量是由于第四关节轴向下俯仰造成，根据所述变换关系建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系。

第四关节角的第一特征角通过以第三姿态的坐标原点为顶点，与第四姿态的坐标原点及第五或第六姿态坐标原点形成的直角三角形中的三角函数求解；

第四关节角的第二特征角通过以以第三姿态的坐标原点为顶点，与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的三角形的三角函数求解。

当机械臂处于初始位型时，θ₄＝＝0，从机械构型中可知θ₄为俯仰角，第四关节角的第二特征角∠ABC固定不变，当关节轴4向上或者向下俯仰时，第四关节角的第一特征角∠B₁BC的大小一直在变化，当第四关节角的第一特征角∠B₁BC变小时，该变化量是由于关节轴4向上俯仰造成，同理当第四关节角的第一特征角∠B₁BC变大时，该变化量是由于关节轴4向下俯仰造成。利用该条件以及关节角极限限制可以得出θ₄，∠B₁BC，∠ABC三者之间的关系。且以第三姿态的坐标原点为顶点，与第四姿态的坐标原点及第五或第六姿态坐标原点形成的直角三角形Rt△B₁BC∠B₁BC＝a tan(B₁C/B₁B)；

在△ABC中，由于|AB|,|BC|,|AC|均已知，根据三角形余弦定理可得∠ABC。

步骤5、由正运动学方程的左右两边的第四列对应相等得到的方程得到关于第一关节角θ₁、第二关节角θ₂、第四关节角θ₄以及第三关节角θ₃的两个等式，从而求出第三关节角θ₃；

具体地，由正运动学方程的左右两边的第四列对应相等可得出

$\left\{\begin{array}{c}{p}_x{c}_1+p_y{x}_1-a_1=d_7(a_x{c}_1+a_y{s}_1)-d_5(c_4{s}_2+c_2{c}_3{s}_4)-d_3{s}_2-a_4{s}_2{s}_4+a_4{a}_2{c}_3{c}_4\\ p_z-d_1=d_7{a}_z+d_5(c_2{c}_4-c_3{s}_2{s}_4)+d_3{c}_2+a_4{c}_2{s}_4+a_4{c}_3{c}_4{s}_2\\ p_x{s}_1-p_y{c}_1=d_7(a_x{s}_1-a_y{c}_1)-a_4{c}_4{s}_3+d_5{s}_3{s}_4\end{array}\right.$

方程式连立可以得到关于θ₁，θ₂，θ₄，θ₃的两个等式，由于前三个角都已知从而求出θ₃；

步骤6、确定位姿矩阵，根据位姿矩阵得到第四姿态的坐标原点坐标，再求出第四姿态的坐标原点和第七姿态的坐标原点之间的空间距离，再由第四姿态的坐标原点、第五或第六姿态的坐标原点和第七姿态的坐标原点构成的三角形中，求出第六关节角的特征角，根据第六关节角的特征角求解第六关节角；其中，第六关节角的特征角为以第五或第六姿态的坐标原点为顶点，与第四姿态的坐标原点、第七姿态的坐标原点形成的夹角；

具体地，在上述步骤1至步骤5求解的关节角已知时，利用正运动学确定的位姿矩阵，该矩阵可以得到第四姿态的坐标原点坐标，且第七姿态的坐标原点已知，通过空间中两点之间距离公式，可求出第四姿态的坐标原点和第七姿态的坐标原点之间的距离，在由第四姿态的坐标原点、第五或第六姿态的坐标原点和第七姿态的坐标原点构成的三角形中，根据三条边长，由由余弦定理可求出第六关节角的特征角，根据第六关节角的特征角求解第六关节角；其中，第六关节角的特征角为以第五或第六姿态的坐标原点为顶点，与第四姿态的坐标原点、第七姿态的坐标原点形成的夹角。

根据第六关节角的特征角求解第六关节角，包括：第六关节角对应为两个解，具体为第六关节角的特征角的补角的正负值。

从第一关节角θ₁至第四关节角θ₄已知时，利用正运动学可确定的位姿矩阵，该矩阵可以得到第四姿态的坐标原点B₁点坐标(x_By_Bz_B)，且第七姿态的坐标原点D点坐标(p_x p_y p_z)′已知，通过空间中两点之间距离公式，可求出|B₁D|，在△B₁CD中，|B₁C|,|CD|均为杆长参数，是已知的，三条边|B₁D|,|B₁C|,|CD|均已知，故由余弦定理可求出∠B₁CD，因此θ₆＝±(π-∠B₁CD)。式中，正，负号对应于θ₆的两个解。

步骤7、根据第六关节角的求解结果，结合末端坐标的关系，分情况求解第五关节角和第七关节角。

根据第六关节角的求解结果，结合末端坐标的关系，分情况求解第五关节角和第七关节角，包括：

当第六关节角不为为零时，由末端坐标的关系求出第五姿态角θ₅，再由末端姿态的关系求出第七姿态角θ₇；

当第六关节角为零时，机械臂处于奇异位置，不能唯一确定第五关节角θ₅和第七关节角θ₇，令第五关节角θ₅＝0，则可求出第七关节角θ₇，得到第五关节角θ₅和第七关节角θ₇的一组特解；

具体地，由于第六关节角时，关节5的轴线和关节7的轴线重合，关节5和关节7都是旋转关节，末端执行器的姿态只于θ₅+θ₇有关，可知θ₅和θ₇的变化对末端贡献是一样的，相当于失去了一个自由度，处于奇异位形，故可分两种情况来求θ₅，θ₇。

(1)当第六关节角θ₆≠0时，由正运动学方程的结果可知，末端坐标(即D点坐标(p_x p_y p_z)′)只与θ₁～θ₆，末端姿态与θ₁～θ₇有关，而末端位姿矩阵为T已知，故可先由末端坐标的关系求出θ₅，再由末端姿态的关系求出θ₇；

(2)当第六关节角θ₆＝0时，机械臂处于奇异位置，这时θ₅，θ₇不能影响末端坐标，只与末端姿态有关，且θ₅，θ₇对末端姿态的影响作用相同，故不能唯一确定θ₅，θ₇，而只能根据末端姿态的关系求出(θ₅+θ₇)，这时可以令θ₅＝0，则可求出θ₇，得到θ₅，θ₇的一组特解。

通过上述步骤可以求出8组解，这8组解中可能存在复数解或者超出关节极限的解，这部分都应该外加限制进行排除，此时选取哪组角最优，要依据上一个离散点求解出来的关节角来判断，即选取与上一次计算最接近的一组角，这样就可以使得关节位移最小，避免出现关节速度超过电机极限限制。

通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本发明可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。

本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于装置或系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的装置及系统实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (7)

1.一种七轴工业机械臂的逆动学求解方法，其特征在于，包括：

步骤1、将虚拟机械臂末端执行器任务设定的起始点和终止点中间对应的位置离散化，获取离散点；

步骤2、建立第一关节角和第七姿态的坐标原点的平面方程，根据每两个离散点得到第一关节角的变化趋势，根据所述变化趋势得到离散点集合中每一点对应的第一关节角的值；

其中，第一平面坐标系为基座底部坐标点、第零姿态的坐标原点以及第一或第二姿态的坐标原点构成的平面；

步骤3、采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵T，根据变换矩阵获取第五或第六姿态的坐标原点，再确定位姿矩阵，根据位姿矩阵求出第三姿态的坐标原点；根据第五或第六姿态的坐标原点和第三姿态的坐标原点的距离方程式，求出第二关节角；

步骤4、建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系，根据所述条件关系求解第四关节角；其中，

所述第四关节角的第一特征角为以第三姿态的坐标原点为顶点，与第四姿态的坐标原点及第五或第六姿态坐标原点形成的夹角；

所述第四关节角的第二特征角以第三姿态的坐标原点为顶点，与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的夹角；

步骤5、由正运动学方程的左右两边的第四列对应相等得到的方程得到关于第一关节角、第二关节角、第四关节角以及第三关节角的两个等式，从而求出第三关节角；

步骤6、确定位姿矩阵，根据位姿矩阵得到第四姿态的坐标原点坐标，再求出第四姿态的坐标原点和第七姿态的坐标原点之间的空间距离，再由第四姿态的坐标原点、第五或第六姿态的坐标原点和第七姿态的坐标原点构成的三角形中，求出第六关节角的特征角，根据第六关节角的特征角求解第六关节角；其中，第六关节角的特征角为以第五或第六姿态的坐标原点为顶点，与第四姿态的坐标原点、第七姿态的坐标原点形成的夹角；

步骤7、根据第六关节角的求解结果，结合末端坐标的关系，分情况求解第五关节角和第七关节角。

2.根据权利要求1所述的一种七轴工业机械臂的逆动学求解方法，其特征在于，在规划第一关节角之前，包括：

以虚拟机械臂末端执行器的以基座的底部点O为基坐标点，再另外依次确定七个位姿状态对应的空间坐标系的坐标原点，其中第一姿态和第二姿态公用同一个坐标原点为第一或第二姿态的坐标原点，第五姿态和第六姿态公用同一个坐标原点为第五或第六姿态的坐标原点。

3.根据权利要求1所述的一种七轴工业机械臂的逆动学求解方法，其特征在于，在建立第一关节角和第七姿态的坐标原点的平面方程之前，旋转第一关节角，将第七姿态的坐标原点旋转至第一平面坐标系内，使第一关节角和第七姿态的坐标原点处于同一平面。

4.根据权利要求3所述的一种七轴工业机械臂的逆动学求解方法，其特征在于，所述建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系，包括：第四关节角的第一特征角与第四关节轴的变换关系：当第四关节角的第一特征角变小时，该变化量是由于第四关节轴向上俯仰造成，同理当第四关节角的第一特征角变大时，该变化量是由于第四关节轴向下俯仰造成，根据所述变换关系建立第四关节角、第 四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系。

5.根据权利要求4所述的一种七轴工业机械臂的逆动学求解方法，其特征在于，包括：

所述第四关节角的第一特征角通过以第三姿态的坐标原点为顶点，与第四姿态的坐标原点及第五或第六姿态坐标原点形成的直角三角形中的三角函数求解；

所述第四关节角的第二特征角通过以以第三姿态的坐标原点为顶点，与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的三角形的三角函数求解。

6.根据权利要求5所述的一种七轴工业机械臂的逆动学求解方法，其特征在于，根据第六关节角的特征角求解第六关节角，包括：第六关节角对应为两个解，具体为第六关节角的特征角的补角的正负值。

7.根据权利要求6所述的一种七轴工业机械臂的逆动学求解方法，其特征在于，根据第六关节角的求解结果，结合末端坐标的关系，分情况求解第五关节角和第七关节角，包括：

当第六关节角不为为零时，由末端坐标的关系求出第五姿态角，再由末端姿态的关系求出第七姿态角；

当第六关节角为零时，机械臂处于奇异位置，不能唯一确定第五关节角和第七关节角，令第五关节角等于零，则可求出第七关节角，得到第五关节角和第七关节角的一组特解。