CN105573143A - 用于六自由度的工业机器人的逆运动学求解方法 - Google Patents

Abstract

本发明提出了一种用于六自由度的工业机器人的逆运动学求解方法，包括：建立连杆坐标系，其中，所述连杆坐标系包括：X-Z轴平面，所述工业机器人的六个关节对应坐标系和所述工业机器人末端执行器的坐标系；在所述连杆坐标系基础上，根据所述工业机器人的六个关节之间的结构几何参数计算欧拉角变换矩阵；利用所述欧拉叫变换矩阵，按照预设顺序依次求解所述工业机器人的六个关节的旋转轴转动角度；根据所述六个关节的旋转轴转动角度对应的八个求解结果，解离上一关节空间所处位置对应的各个关节旋转轴角度差值范数总和，依据范数综合最小原则选择最优解。本发明具有求解精度高、求解速度快、求解过程更加简单易懂，计算量小的特点。

Description

用于六自由度的工业机器人的逆运动学求解方法

技术领域

本发明涉及无人机技术领域，特别涉及一种用于六自由度的工业机器人的逆运动学求解方法。

背景技术

工业机器人的逆向运动学问题是在给定执行器末端相对于基座的位置和姿态，以及所有连杆几何参数的情况下，求取所有关节转动角度值，是正向运动学的逆过程。正向运动学根据几何参数可以得到前后相邻关节之间的欧拉角变换矩阵T，当知道每个关节转动角度θ，通过变换矩阵T简单运算即可得到末端执行器的位置和姿态，且解唯一；而逆向运动学的求解则相对复杂，并且可能出现多解或无解的情况。

现在市场上大部分六自由度工业机器人都是属于Pieper提出的一类腕部相邻的三个关节旋转轴相交于同一点(俗称腕点)的工业机器人，现有的通用坐标系建模都是采用DH参数法，同时现有逆运动学求解方法大多数都是利用代数法或几何分析法单独实现的。

现有工业机器人的逆运动学求解主要存在缺陷及不足：针对特殊构型机器人，通用求解方法坐标系建模方法单一、求解过程复杂难懂、计算量大且求解速度慢。

发明内容

本发明的目的旨在至少解决所述技术缺陷之一。

为此，本发明的目的在于提出一种用于六自由度的工业机器人的逆运动学求解方法，具有求解精度高、求解速度快、求解过程更加简单易懂，计算量小的特点。

为了实现上述目的，本发明的实施例提供一种用于六自由度的工业机器人的逆运动学求解方法，包括如下步骤：

步骤S1，建立连杆坐标系，其中，所述连杆坐标系包括：X-Z轴平面，所述工业机器人的六个关节对应坐标系和所述工业机器人末端执行器的坐标系；

步骤S2，在所述连杆坐标系基础上，根据所述工业机器人的六个关节之间的结构几何参数计算欧拉角变换矩阵；

步骤S3，利用所述欧拉叫变换矩阵，按照预设顺序依次求解所述工业机器人的六个关节的旋转轴转动角度；

步骤S4，根据所述六个关节的旋转轴转动角度对应的八个求解结果，解离上一关节空间所处位置对应的各个关节旋转轴角度差值范数总和，依据范数综合最小原则选择最优解。

进一步，在所述步骤S2中，

根据所述工业机器人的六个关节之间的结构几何参数d₁计算姿态变换矩阵和位置变换矩阵其中i＝0，1，2，…，9；

计算坐标系{j}相对于坐标系{i}的变换矩阵

进一步，所述工业机器人的六个关节间的变换矩阵满足如下：

$T_6^0=T_1^{0}T_2^{1}T_3^{2}T_4^{3}T_5^{4}T_6^5,$

其中，为末端执行器的坐标系相对于基础坐标系的变换矩阵，为第一关节的坐标系相对于基础坐标系的变换矩阵，为第二关节的坐标系相对于第一关节坐标系的变换矩阵；为第三关节的坐标系相对于第二关节坐标系的变换矩阵；为第四关节的坐标系相对于第三关节坐标系的变换矩阵；为第五关节的坐标系相对于第四关节坐标系的变换矩阵；为第六关节的坐标系相对于第五关节坐标系的变换矩阵。

进一步，在所述步骤S3中，依次计算所述工业机器人的第一关节、第五关节、第六关节、第三关节、第二关节和第四关节的旋转轴转动角度。

根据本发明实施例的用于六自由度的工业机器人的逆运动学求解方法，针对该类特殊构型工业机器人，坐标系建模方法简单易懂。并且利用几何分析法和欧拉角变换法联合求解机器人逆运动学解，能够保证该类工业机器人逆运动学解的求解精度。本发明提出的逆运动学求解过程相比于单一代数法和几何分析法要简单许多，能够保证该类工业机器人逆运动学求解速度，求解速度快。本发明中提出的逆运动学求解过程更加简单易懂，计算量小。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本发明实施例的用于六自由度的工业机器人的逆运动学求解方法的流程图；

图2为根据本发明实施例的六自由度特殊构型工业机器人的二维平面图；

图3为根据本发明实施例的坐标系{5}的原点在{0}坐标系的X-Y平面的投影示意图；

图4为根据本发明实施例的都坐标系{1}的X-Y平面示意图；

图5为根据本发明实施例的坐标系{1}的Y-Z平面示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本发明提出一种用于六自由度的工业机器人的逆运动学求解方法，利用几何分析法和欧拉角变换法结合求解该类特殊构型工业机器人逆运动学解的方法。该类工业机器人特殊构型在于坐标系{3}，{4}，{5}的旋转轴不相交于同一点，这种机器人腕部不存在奇异性。

如图1所示，本发明实施例的用于六自由度的工业机器人的逆运动学求解方法，包括如下步骤：

步骤S1，建立连杆坐标系

如图2所示，连杆坐标系包括：X-Z轴平面，Y轴根据右手法则确定，工业机器人的六个关节对应坐标系{0}，{1}，{2}，{3}，{4}，{5}，其中{0}为固定基础坐标系，工业机器人末端执行器的坐标系{6}。图2中带箭头射线围绕的坐标轴为相应关节旋转轴，依次设该类特殊构型工业机器人的六个关节旋转角度分别为变量θ₁，θ₂，θ₃，θ₄，θ₅，θ₆。

步骤S2，在连杆坐标系基础上，根据工业机器人的六个关节之间的结构几何参数计算欧拉角变换矩阵。

根据工业机器人的六个关节之间的结构几何参数d₁计算姿态变换矩阵和位置变换矩阵其中i＝0，1，2，…，9；

计算坐标系{j}相对于坐标系{i}的变换矩阵

工业机器人的六个关节间的变换矩阵满足如下：

$T_6^0=T_1^{0}T_2^{1}T_3^{2}T_4^{3}T_5^{4}T_6^5,---\left(1\right)$

其中，为末端执行器的坐标系相对于基础坐标系的变换矩阵，为第一关节的坐标系相对于基础坐标系的变换矩阵，为第二关节的坐标系相对于第一关节坐标系的变换矩阵；为第三关节的坐标系相对于第二关节坐标系的变换矩阵；为第四关节的坐标系相对于第三关节坐标系的变换矩阵；为第五关节的坐标系相对于第四关节坐标系的变换矩阵；为第六关节的坐标系相对于第五关节坐标系的变换矩阵。

步骤S3，利用欧拉叫变换矩阵，按照预设顺序依次求解工业机器人的六个关节的旋转轴转动角度。

在本发明的一个实施例中，依次计算工业机器人的第一关节、第五关节、第六关节、第三关节、第二关节和第四关节的旋转轴转动角度。

第一步，分析该特殊构型机器人几何结构及投影关系，求解逆运动学解的θ₁。

设坐标系{0}的原点到坐标系{5}原点矢量分离出 $T_6^5=\left[\begin{array}{cc}R_6^5& P_6^5\\ 0& 1\end{array}\right],$ 其中为3x3的旋转矩阵，得到等式 $\left[\begin{array}{c}P_5^0\\ 0\end{array}\right]=T_6^0\left[\begin{array}{c}-d_7\\ 0\\ 0\\ 1\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right].$

根据工业机器人逆运动学解法，及几何尺寸均已知。

参考图3，计算得到

根据几何对称特性，θ₁有两个解，分别对应工业机器人领域中的“左肩”，“右肩”。

第二步，求解逆运动学解的θ₅。

参考图4，以坐标系{1}为参考基础坐标系。坐标系{4}旋转θ₅，坐标系{5}的中d₇长度在坐标系{4}的X轴方向的投影长度为d₇cos(θ₅)，然后得到关系式，

${P_6^1}_x=d_7\cos \left({\mathrm{\θ}}_5\right)-d_9,{P_6^1}_x=\cos \left({\mathrm{\θ}}_1\right){P_6^0}_x+\sin \left({\mathrm{\θ}}_1\right){P_6^0}_y-d_1,---\left(3\right)$

联立求解公式(3)，得到 ${\mathrm{\θ}}_5=\±\arccos \left(\frac{\cos \left({\mathrm{\θ}}_1\right){P_6^0}_x+\sin \left({\mathrm{\θ}}_1\right){P_6^0}_y-d_8}{d_7}\right),---\left(4\right)$

θ₅有两个解，分别对应工业机器人领域中的“腕上”，“腕下”。

第三步，求解逆运动学解的θ₆。

求解出关节转动角θ₁，θ₅后，可以求出再根据坐标系{1}，{2}，{3}的X轴方向始终平行的特点，可以得到下面两个关系式：

cos(θ₁)r₁₂+sin(θ₁)r₂₂＝-sin(θ₅)cos(θ₆)，(5)

cos(θ₁)r₁₃+sin(θ₁)r₂₃＝sin(θ₅)sin(θ₆)，(6)

联立上述两个关系式(5)和(6)，推导得到即得到

${\mathrm{\θ}}_6=\arctan 2\[\frac{cos\left({\mathrm{\θ}}_1\right)r_{13}+sin\left({\mathrm{\θ}}_1\right)r_{23}}{-(cos\left({\mathrm{\θ}}_1\right)r_{12}+sin\left({\mathrm{\θ}}_1\right)r_{22})}\],---\left(7\right)$

但当sin(θ₅)＝0或者(1)＝0，(2)＝0时，θ₆是没有明确定义的。

第四步，求解逆运动学解的θ₃及θ₂。

结合以上已经求解出的关节转动角度θ₁，θ₅和θ₆，根据欧拉角变换法，可以得到

$T_4^1=T_2^{1}T_3^{2}T_4^3={\left(T_1^0\right)}^{1}T_6^0{\left(T_6^5\right)}^{-1}{\left(T_5^4\right)}^{-1},---\left(8\right)$

参考图5，OA＝d₂，AB＝d₄，

得到等式

${\mathrm{cos\θ}}_3=\frac{\[{\left({P_4^1}_y\right)}^2+{\left({P_4^1}_z\right)}^2\]-({d_2}^2+{d_4}^2)}{2d_2{d}_4},$

然后计算得到 ${\mathrm{\θ}}_3=\±\arccos \left\{\frac{\[{\left({P_4^1}_y\right)}^2+{\left({P_4^1}_z\right)}^2\]-({d_2}^2+{d_4}^2)}{2d_2{d}_4}\right\},---\left(9\right)$

θ₃有两个解，分别对应工业机器人领域中的“上肘”，“下肘”。、

根据正弦定理，可以得到以下关系式：参考图5得到根据关节二转动角度θ₂＝η-β关系式，求解得到

${\mathrm{\θ}}_2=-\arctan 2\left(\frac{{P_4^1}_y}{{P_4^1}_z}\right)-\arcsin \left(\frac{d_{4}sin\left({\mathrm{\θ}}_3\right)}{\left|OB\right|}\right),---\left(10\right)$

第五步，求解逆运动学解的θ₄。

根据前几步已经求解出各关节转动角度，求出变换矩阵

$T_4^1=T_2^{1}T_3^{2}T_4^3\⇒T_4^3={\left(T_3^2\right)}^{-1}{\left(T_2^1\right)}^{-1}T_4^1,$ 利用变换矩阵 $T_4^3=\left[\begin{array}{cccc}1& 0& 0& d_5\\ 0& cos\left({\mathrm{\θ}}_4\right)& -sin\left({\mathrm{\θ}}_4\right)& 0\\ 0& sin\left({\mathrm{\θ}}_4\right)& \cos \left({\mathrm{\θ}}_4\right)& 0\\ 0& 0& 0& 1\end{array}\right],$ 推导得到其中r₃₂，r₂₂为欧拉角变换法求出的旋转矩阵R对应的两项，得到

${\mathrm{\θ}}_4=\arctan 2\left(\frac{r_{32}}{r_{22}}\right),---\left(11\right)$

步骤S4，根据六个关节的旋转轴转动角度对应的八个求解结果，解离上一关节空间所处位置对应的各个关节旋转轴角度差值范数总和，依据范数综合最小原则选择最优解。

从步骤S3中求出的八组可能的逆运动学解中选择最优解，根据八组可能的解离起始(上一关节空间所处位置，即六个转动轴旋转角度θ₁，θ₂，θ₃，θ₄，θ₅，θ₆)对应各关节旋转轴角度差值的范数总和最小值原则进行选择。

根据本发明实施例的用于六自由度的工业机器人的逆运动学求解方法，针对该类特殊构型工业机器人，坐标系建模方法简单易懂。并且利用几何分析法和欧拉角变换法联合求解机器人逆运动学解，能够保证该类工业机器人逆运动学解的求解精度。本发明提出的逆运动学求解过程相比于单一代数法和几何分析法要简单许多，能够保证该类工业机器人逆运动学求解速度，求解速度快。本发明中提出的逆运动学求解过程更加简单易懂，计算量小。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求极其等同限定。

Claims (4)

1.一种用于六自由度的工业机器人的逆运动学求解方法，其特征在于，包括如下步骤：

步骤S1，建立连杆坐标系，其中，所述连杆坐标系包括：X-Z轴平面，所述工业机器人的六个关节对应坐标系和所述工业机器人末端执行器的坐标系；

步骤S2，在所述连杆坐标系基础上，根据所述工业机器人的六个关节之间的结构几何参数计算欧拉角变换矩阵；

步骤S3，利用所述欧拉叫变换矩阵，按照预设顺序依次求解所述工业机器人的六个关节的旋转轴转动角度；

步骤S4，根据所述六个关节的旋转轴转动角度对应的八个求解结果，解离上一关节空间所处位置对应的各个关节旋转轴角度差值范数总和，依据范数综合最小原则选择最优解。

2.如权利要求1所述的用于六自由度的工业机器人的逆运动学求解方法，其特征在于，在所述步骤S2中，

根据所述工业机器人的六个关节之间的结构几何参数d_i计算姿态变换矩阵和位置变换矩阵其中i＝0，1，2，...，9；

计算坐标系{j}相对于坐标系{i}的变换矩阵

3.如权利要求2所述的用于六自由度的工业机器人的逆运动学求解方法，其特征在于，所述工业机器人的六个关节间的变换矩阵满足如下：

$T_6^0=T_1^{0}T_2^{1}T_3^{2}T_4^{3}T_5^{4}T_6^5,$

其中，为末端执行器的坐标系相对于基础坐标系的变换矩阵，为第一关节的坐标系相对于基础坐标系的变换矩阵，为第二关节的坐标系相对于第一关节坐标系的变换矩阵；为第三关节的坐标系相对于第二关节坐标系的变换矩阵；为第四关节的坐标系相对于第三关节坐标系的变换矩阵；为第五关节的坐标系相对于第四关节坐标系的变换矩阵；为第六关节的坐标系相对于第五关节坐标系的变换矩阵。

4.如权利要求1所述的用于六自由度的工业机器人的逆运动学求解方法，其特征在于，在所述步骤S3中，依次计算所述工业机器人的第一关节、第五关节、第六关节、第三关节、第二关节和第四关节的旋转轴转动角度。