CN102785248A - 一种解耦型六自由度工业机器人的运动控制方法 - Google Patents

Abstract

本发明公开了一种解耦型六自由度工业机器人的运动控制方法，该方法包括：（a）根据机器人所需实现的位姿，通过D-H模型法获得末端执行机构相对于基坐标系的位姿矩阵；（b）将机器人避开奇异形位时所能实现的正常位姿定义为不同的关节特性属性，并设定机器人实现所需的位姿时的关节特征属性组合；（c）根据位姿矩阵以及设定的关节特征属性组合及其取值条件，通过反变换法分别求得关于机器人各个关节变量的唯一解；（d）根据所求得的解，执行对六自由度工业机器人的关节运动，相应完成整体运动控制过程。通过本发明，具备可预知过奇异点路径、算法简单、反解速度快以及能较好地确定唯一解等优点，并能很好地应用于实际的工业机器人运动控制。

Description

一种解耦型六自由度工业机器人的运动控制方法

技术领域

本发明属于工业机器人运动学技术领域，更具体地，涉及一种解耦型六自由度工业机器人的运动控制方法。

背景技术

工业机器人是面向工业领域的多关节机械手或多自由度的机器人。从外形来看，它是由一系列刚性连杆通过一系列柔性关节交替连接而成的开式链结构。在机器人的运动控制中，需要根据给定的机器人末端执行器的位置和姿态，求解机器人各关节的关节变量，这个求解过程称为机器人的操作臂运动学反解。机器人的操作臂运动学反解是机器人离线编程和轨迹规划的前提，是机器人运动控制的基础。

在进行机器人操作臂运动学反解时，常遇到的一个问题为其解并非是唯一解。机器人操作臂运动学反解的数目一般取决于关节数目、连杆参数和关节变量的活动范围。对于全部为旋转关节的6自由度机器人来说，可能多达16种解，而对于解耦6自由度机器人（前三个关节为转动关节，后三个关节轴线相交于一点）来说最多有8种解。但每个机器人系统最终只能选择一个解，因此机器人的多解现象会给运动控制带来一些问题。

运动学反解的选择没有统一的标准，在实际应用中，一般根据机器人实际结构选取其中最优的一组解（如行程最短、功率最省、受力最好、回避障碍等），建立对反解值进行划分的规范。在不考虑避障的情况下，一个常用的比较合理的选择是选取“最短行程”的解，即在逆解空间中搜索每个关节变量都最小的一组解，相应可采用的搜索计算式为：

Min{(θ₁-θ₁')²+(θ₂-θ₂')²+(θ₃-θ₃')²+(θ₄-θ₄')²+(θ₅-θ₅＇)²+(θ₆-θ₆')²}

其中，θ₁~θ₆表示求出的多组反解，θ₁′~θ₆′表示各组解的平均值。在没有障碍物时，搜索到的最优逆解就是关节空间中最接近运动起始点的逆解。

也常有人采用“多移动小关节、少移动大关节”的原则，在六轴机器人中，大关节一般指前三个关节，小关节一般指后三个关节，因此在逆解空间中选取前三个关节变量小、后三个关节变量大的一组逆解，可用下式进行判断（各变量意义与上式中相同）：

Max{(|θ₄-θ₄′|+|θ₅-θ₅′|+|θ₆-θ₆′|)/(|θ₁-θ₁′|+|θ₂-θ₂′|+|θ₃-θ₃′|)}

可以看出，现有技术中的机器人运动学反解方法的算法都比较复杂，因此具有运算量大、实现不易的缺点。在CN200810106444.8的专利申请中，公开了一种六自由度机器人运动学CORDIC算法协处理器及其相应的处理方式，其中通过采用CORDIC算法替代了传统的通过计算大量超越方程才能完成机器人运动学正反解计算，使大量的超越方程计算过程简化为只需要通过加法、减法、和移位等这些硬件便于实现的操作，从而大大提高了协处理器的计算速度；然而，其协处理器包括运算模块、逻辑模块及大量输入输出接口，虽然算法简单但实现较繁琐。

发明内容

针对现有技术的缺陷和技术需求，本发明的目的在于提供一种解耦型六自由度工业机器人的运动控制方法，该方法具有可预知过奇异点路径、算法简单、反解速度快，以及能较好地确定唯一解等优点，并能很好地应用于实际的工业机器人运动控制中。

按照本发明，提供了一种解耦型六自由度工业机器人的运动控制方法，该六自由度工业机器人包括底座、腰部回转部件、大臂、小臂、手腕部件和末端执行机构，并通过第一关节完成机器人腰部的回转运动，通过第二、第三关节分别执行大臂和小臂的俯仰动作，以及通过第四至第六关节共同执行末端执行机构的位姿操作，所述方法包括下列步骤：

（a）根据对机器人末端执行机构所需实现的位置和姿态，通过D-H模型法获得末端执行机构相对于基坐标系的位姿矩阵

$T_6^0=T_1^{0}T_2^{1}T_3^{2}T_4^{3}T_5^{4}T_6^5=\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]---\left(1\right)$

该位姿矩阵中各个元素分别如以下表达式组（2）所示，其中c₁~c₆、s₁~s₆分别依次表示机器人各个关节也即第一至第六关节的变量θ₁~θ₆的余弦值和正弦值，c₂₃、s₂₃分别表示第二和第三关节的变量θ₂与θ₃之和的余弦值和正弦值，a₁~a₃分别表示机器人第一、第二和第三关节各自与其相邻的下一关节之间的轴线距离，d₄表示第三关节的轴线与第四、五关节轴线交点之间的距离：

$\left.\begin{array}{c}{n}_x=c_1[c_{23}(c_4{c}_5{c}_6-s_4{s}_6)-s_{23}{s}_5{c}_6]+s_1(s_4{c}_5{c}_6+c_4{s}_6)\\ n_y=s_1[c_{23}(c_4{c}_5{c}_6-s_4{s}_6)-s_{23}{s}_5{c}_6]-c_1(s_4{c}_5{c}_6+c_4{s}_6)\\ n_z=s_{23}(c_4{c}_5{c}_6-s_4{s}_6)+c_{23}{s}_5{c}_6\\ o_x=c_1[c_{23}(-c_4{c}_5{c}_6-s_4{c}_6)+s_{23}{s}_5{s}_6]+s_1(c_4{c}_6-s_4{c}_5{s}_6)\\ o_y=s_1[c_{23}(-c_4{c}_5{c}_6-s_4{c}_6)+s_{23}{s}_5{s}_6]-c_1(c_4{c}_6-s_4{c}_5{s}_6)\\ o_z=s_{23}(-c_4{c}_5{c}_6-s_4{c}_6)-c_{23}{s}_5{s}_6\\ a_x=c_1(c_{23}{c}_4{s}_5+s_{23}{c}_5)+s_1{s}_4{s}_5\\ a_y=s_1(c_{23}{c}_4{s}_5+s_{23}{c}_5)-c_1{s}_4{s}_5\\ a_z=s_{23}{c}_4{s}_5-c_{23}{c}_5\\ p_x=c_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ p_y=s_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ p_z=a_2{s}_2+a_3{s}_{23}-d_4{c}_{23}\end{array}\right\}---\left(2\right)$

（b）将机器人避开奇异形位时所能实现的正常位姿定义为不同的关节特性属性cfg1、cfg2和cfg3，并设定机器人末端执行机构实现所需的位姿时的关节特征属性组合，其中所述关节特性属性所代表的意义和取值条件分别如下：

cfg1表示当机器人手腕部件的中心点分别处于第一参考平面左右两侧时的状态，并且满足 $\mathrm{cfg}1=\left\{\begin{array}{c}1,c_1{p}_x+s_1{p}_y\&GreaterEqual;0\\ 0,c_1{p}_x+s_1{p}_y<0\end{array}\right.,$ 所述第一参考平面是以机器人第一关节的转动轴线与第二关节的轴线平行线两者所构成的平面；

cfg2表示机器人手腕部件的中心点分别处于第二参考平面左右两侧时的状态，并且满足 $\mathrm{cfg}2=\left\{\begin{array}{c}1,\tan {\mathrm{\&theta;}}_3\&GreaterEqual;d_4/a_3\\ 0,\tan {\mathrm{\&theta;}}_3<d_4/a_3\end{array}\right.,{\mathrm{\&theta;}}_3\&Element;(-\mathrm{\&pi;},\mathrm{\&pi;}),$ 所述第二参考平面是以机器人大臂和第三关节的轴线所构成的平面；

cfg3表示机器人手腕部件的中心点分别处于第三参考平面左右两侧时的状态，并且满足 $\mathrm{cfg}3=\left\{\begin{array}{c}1,{\mathrm{\&theta;}}_5<0\\ 0,{\mathrm{\&theta;}}_5\&GreaterEqual;0\end{array}\right.,{\mathrm{\&theta;}}_3\&Element;(-2\mathrm{\&pi;},2\mathrm{\&pi;}),$ 所述第三参考平面是以机器人小臂和第五关节的轴向所构成的平面；

（c）根据步骤（a）所获得位姿矩阵、以及步骤（b）所设定的关节特征属性组合及其取值条件，通过反变换法分别求得关于机器人各个关节变量θ₁~θ₆的唯一解；

（d）根据所求得的各个关节变量的解，执行对六自由度工业机器人的关节运动，相应完成所需位姿的运动控制过程。

作为进一步优选地，所述反变换法求解机器人各个关节变量的过程具体包括以下步骤：

（c1）根据表达式θ₁=Atan2(p_y,p_x)，求出关于关节变量θ₁的两个解，并依照所设定的所述关节特征参数cfg1，从这两个解中获得关于关节变量θ₁的唯一解；

（c2）利用步骤（c1）所求得的关于关节变量θ₁的唯一解，并根据以下表达式组求出关于关节变量θ₃的两个解，并依照所设定的所述关节特征参数cfg2，从这两个解中获得关于关节变量θ₃的唯一解：

${\mathrm{\&theta;}}_3=A\tan 2(k,\&PlusMinus;\sqrt{{a_3}^2+{d_4}^2-k^2})-A\tan 2(a_3,d_4)$

$k=\frac{{(c_1{p}_x+s_1{p}_y-a_1)}^2+{p_z}^2-{a_2}^2-{a_3}^2-{d_4}^2}{2a_2}$

（c3）利用所求得的关于关节变量θ₁、θ₃的唯一解，根据以下表达式直接求出关于关节变量θ₂的唯一解：

θ₂=Atan2[(a₃+a₂c₃)p_z+(c₁p_x+s₁p_y-a₁)(a₂s₃+d₄),-(d₄+a₂s₃)p_z+(c₁p_x+s₁p_y-a₁)(a₂c₃+a₃)]-θ₃

（c4）利用所求得的关于关节变量θ₁~θ₃的唯一解，根据以下表达式求出关于关节变量θ₅的两个解，同时依照所设定的所述关节特征参数cfg3，从这两个解中获得关于关节变量θ₅的唯一解；

（c5）利用所求得的关于关节变量θ₁~θ₃、θ₅的唯一解，根据以下表达式分别求出关于关节变量θ₄和θ₆的唯一解：

${\mathrm{\&theta;}}_4=\left\{\begin{array}{c}A\tan (a_x{s}_1-a_y{c}_1,a_x{c}_1{c}_{23}+a_y{s}_1{c}_{23}+a_z{s}_{23}),{\mathrm{\&theta;}}_5>0\\ A\tan (a_y{c}_1-a_x{s}_1,-a_x{c}_1{c}_{23}-a_y{s}_1{c}_{23}-a_z{s}_{23}),{\mathrm{\&theta;}}_5<0\end{array}\right.$

${\mathrm{\&theta;}}_6=\left\{\begin{array}{c}A\tan (o_x{c}_1{s}_{23}+o_y{s}_1{s}_{23}-o_z{c}_{23},-n_x{c}_1{s}_{23}-n_y{s}_1{s}_{23}+n_z{c}_{23}),{\mathrm{\&theta;}}_5>0\\ A\tan (-o_x{c}_1{s}_{23}-o_y{s}_1{s}_{23}+o_z{c}_{23},n_x{c}_1{s}_{23}+n_y{s}_1{s}_{23}-n_z{c}_{23}),{\mathrm{\&theta;}}_5<0\end{array}\right..$

总体而言，按照本发明的六自由度工业机器人运动控制方法与现有技术相比，主要具备以下的优点：

1、不仅可省去复杂的比较择优过程，提高运动学反解速度，而且还能预知机器人运动中的过奇异点路径和无解路径；

2、运算效率高、易于编程实现，通过对较多机器人实例的运动学正反解的对照验证表明，可实现对机器人关节变量的唯一解的正确反解，相应实现对机器人所需位姿的适当运动控制；

3、在按照本发明的运动学反解过程中，通过建立末端执行机构的位姿矩阵、使用机器人关节变量，以及采用特征属性来表征机器人避开奇异形位时的不同正常位姿表征等技术手段，相应能够取得便于执行运动学反解过程、对机器人顺利获得所需位姿等技术效果，因此在本领域具备现实的运用前景。

附图说明

图1是显示六自由度工业机器人整体组成及连接关系的结构示意图；

图2a-2h是对六自由度工业机器人执行运动学反解的8种结果的示意图；

图3a-3c是按照本发明显示六自由度工业机器人的一种奇异形位、以及根据该奇异形位来相应划分机器人处于非奇异形位时的2种位置状态的示意图；

图4a-4c是按照本发明显示六自由度工业机器人的另外一种奇异形位、以及根据该奇异形位来相应划分机器人处于非奇异形位时的2种位置状态的示意图；

图5a-5c是按照本发明显示六自由度工业机器人的第三种奇异形位、以及根据该奇异形位来相应划分机器人处于非奇异形位时的2种位置状态的示意图；

图6是按照本发明的用于对六自由度机器人的运动控制方法的流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

图1是显示六自由度工业机器人整体组成及连接关系的结构示意图。参见图1，六自由度工业机器人通常包括底座10、腰部回转部件20、大臂30、小臂40、手腕部件50和末端执行机构60。底座10譬如固定在自动引导车上，并支撑整个装置。腰部回转部件20设置在底座10上支撑大臂30，同时通过第一关节完成机器人腰部的回转运动。大臂30与腰部回转部件20通过第二关节相连地设置，并能够执行俯仰动作。小臂40与大臂30通过第三关节相连地设置，相应可执行俯仰动作。手腕部件50与小臂40相连并在其上安装有末端执行机构60，同时通过第四至第六关节共同进行对末端执行机构60的姿态改变及相应操作。如图1中所示，整个机器人包括六个关节，其中前三个关节决定了末端执行机构在空间的位置，后三个关节决定了末端执行机构在空间的姿态。因此，如果已知末端执行机构的位姿，对这六个关节的变量进行求解即为机器人运动学反解过程。

图2是对解耦型六自由度工业机器人执行运动学反解的8种结果的示意图。如图2中所示，对于末端执行机构60最终达到的同一个空间位姿，这种解耦类型机器人可以通过8种方式来实现，例如图2a中的大臂仰起小臂俯下的状态，与图2g中的大臂俯下小臂仰起的状态一样都是最终达到同一个空间位姿，这也代表了该机器人具有8种运动学反解。

为了更好地获得唯一的反解，本发明从奇异形位的角度出发，来考虑对机器人关节状态的描述，这样在做运动学反解时，只要预先知道能够准确区分机器人关节状态的特征属性，就能唯一确定各关节角，做到关节空间到位姿空间的一一对应。

对于解耦型六自由度工业机器人而言，包括三种奇异形位。当机器人处于奇异形位时，此时机构速度反解不存在具有某些不可控的自由度，另外当机构处于奇异形位附近时，关节驱动力将趋于无穷大从而造成机器人的损坏，因此在设计和应用机器人时应避开奇异形位。相应地，可以以每种奇异形位作为临界点来把机器人划分为两种状态，这样根据三种奇异形位共可把机器人划分为8种正常运动状态。

如图3b中所示，当机器人手腕部件的中心点位于关节1的转动轴线上时，无论关节1怎样旋转，手腕部件50中心点的位置保持不变，机器人将丧失一个自由度，此为机器人的第一个奇异形位，可称为机器人的位置奇点。可以根据此奇异点来划分机器人的第一个关节特征属性cfg1，即“前/后”关节属性：具体而言，以关节1的转动轴线与关节2的轴线平行线两者所成的平面作为参考平面，如图3a和3c中分别所示地，当手腕部件50的中心点分别处于该参考平面左右两侧时，即定义机器人的关节特征属性为“前”和“后”。

如图4b中所示，当大臂30和小臂40处于同一直线上时，机器人雅可比矩阵降秩，手腕部件50的中心点仅能沿切向运动，丧失径向自由度，此为机器人的第二个奇异形位，可称为机器人的速度奇点。可根据此奇异点划分机器人的第二个关节特征属性cfg2，即“上/下”关节属性：具体而言，以大臂30和关节3的轴线所成的平面作为参考平面，如图4a和4c中分别所示，当手腕部件的中心点处于该参考平面左右两侧时，即分别定义机器人的第二个关节特征属性为“上”和“下”。

此外，如图5b中所示，当关节4和关节6的转动轴线重合时，其运动互相抵消，机器人也丧失一个自由度，此为机器人的第三个奇异形位，可称为机器人的姿态奇点。可根据此奇异点划分机器人的第三个关节属性cfg3，即“俯/仰”属性：具体而言，以小臂40和关节5的轴线所成的平面作为参考面，如图5a和5b中分别所示，当手腕部件50的中心点处于该参考平面左右两侧时，即分别定义机器人的第三个关节特征属性为“俯”和“仰”。

以上三个关节特征属性可以组合为“前下俯”、“前下仰”、“前上俯”、“前上仰”、“后上俯”、“后上仰”、“后下俯”、“后下仰”8种组合，这也代表了机器人在能够避开奇异形位的情况下所能实现的8种正常位姿。

相应地，能够利用机器人的8种正常位姿来做机器人运动学的反解，即可根据指定的关节特征属性来选取符合条件的唯一解。引入关节特征属性后，机器人运动学反解具备如下特点：1）除了处于属性临界点以外，机器人运动学反解集合都能被这两种状态平分，也就是说反解得到的机器人关节组合中，如果有若干组落在某属性的一个状态中，则必有相同数目的关节组合落在该属性的另一个状态中；2）对机器人位姿空间中的两个不同位姿做反解计算时，对于所得到的两组解之间，同属性下的机器人关节移动量小于异属性下的机器人关节移动量；3）根据第2个特点以及行程最短原则可以看出，机器人在位姿空间中的运动将基本保持初始状态下的属性，因此在具体运动控制过程中，机器人做线性或圆弧运动时会尽量避免关节特征属性的临界点也即奇异点。

下面将参照图6，来具体描述按照本发明引入关节特征属性后的六自由度工业机器人的运行控制流程。

首先，为了描述末端执行机构在空间的位置和姿态，可以在每个关节上建立一个坐标系，利用坐标系之间的关系来描述末端执行机构的位姿。常用的建立坐标系的方法有多种，例如矩阵变换法、五参数法、四参数法等。其中四参数法（D-H模型法）利用齐次变换描述机器人各个连杆相对于固定参考系的空间几何关系，用一个4×4的齐次变换矩阵描述相邻两连杆的空间关系，从而可推导出末端执行机构的坐标系相对于基坐标系的等价齐次坐标变换矩阵，建立操作臂的运动方程。

因此，如图6中所示，在流程第一步中，根据已知或所需实现的机器人末端执行机构所处位置和姿态，以及各连杆参数在内的固有信息，相应能够在六自由度工业机器人的每个关节上建立相应的坐标系（譬如通过D-H模型法），由此获得机器人末端执行机构相对于基坐标系（也即固结在基座上的坐标系）的位姿矩阵

$T_6^0=T_1^{0}T_2^{1}T_3^{2}T_4^{3}T_5^{4}T_6^5=\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]---\left(1\right)$

对于表达式（1），其中矩阵中各个元素分别可表示为：

$\left.\begin{array}{c}{n}_x=c_1[c_{23}(c_4{c}_5{c}_6-s_4{s}_6)-s_{23}{s}_5{c}_6]+s_1(s_4{c}_5{c}_6+c_4{s}_6)\\ n_y=s_1[c_{23}(c_4{c}_5{c}_6-s_4{s}_6)-s_{23}{s}_5{c}_6]-c_1(s_4{c}_5{c}_6+c_4{s}_6)\\ n_z=s_{23}(c_4{c}_5{c}_6-s_4{s}_6)+c_{23}{s}_5{c}_6\\ o_x=c_1[c_{23}(-c_4{c}_5{c}_6-s_4{c}_6)+s_{23}{s}_5{s}_6]+s_1(c_4{c}_6-s_4{c}_5{s}_6)\\ o_y=s_1[c_{23}(-c_4{c}_5{c}_6-s_4{c}_6)+s_{23}{s}_5{s}_6]-c_1(c_4{c}_6-s_4{c}_5{s}_6)\\ o_z=s_{23}(-c_4{c}_5{c}_6-s_4{c}_6)-c_{23}{s}_5{s}_6\\ a_x=c_1(c_{23}{c}_4{s}_5+s_{23}{c}_5)+s_1{s}_4{s}_5\\ a_y=s_1(c_{23}{c}_4{s}_5+s_{23}{c}_5)-c_1{s}_4{s}_5\\ a_z=s_{23}{c}_4{s}_5-c_{23}{c}_5\\ p_x=c_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ p_y=s_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ p_z=a_2{s}_2+a_3{s}_{23}-d_4{c}_{23}\end{array}\right\}---\left(2\right)$

对于表达式（2），其中各个符号的代表含义如下：c₁~c₆、s₁~s₆分别依次表示机器人各个关节变量θ₁~θ₆的余弦值和正弦值，c₂₃、s₂₃分别表示关节变量θ₂与θ₃之和的余弦值和正弦值，a₁~a₃分别表示机器人第一、第二和第三关节各自与其相邻的下一关节之间的轴线距离，d₄表示第三关节轴线和第四、五关节轴线交点之间的距离。

接着，在依据机器人末端执行机构的位姿矩阵来执行运动学反解之前，根据工艺条件或其他需求预先设定机器人的关节特征属性，以便机器人能够在避开奇异形位的情况下实现正常位姿。

具体而言，关节特征属性cfg1、cfg2和cfg3可分别被赋值为0或1，以区别机器人各个关节处于“前下俯”、“前下仰”、“前上俯”、“前上仰”、“后上俯”、“后上仰”、“后下俯”、“后下仰”8种组合中的哪种位姿。在本发明中，可以设定关节特征属性呈“前”、“上”、“俯”时为1，而关节特征属性呈“后”、“下”、“仰”时为0，则有：

$\mathrm{cfg}1=\left\{\begin{array}{c}1,c_1{p}_x+s_1{p}_y\&GreaterEqual;0\\ 0,c_1{p}_x+s_1{p}_y<0\end{array}\right.---\left(3\right)$

$\mathrm{cfg}2=\left\{\begin{array}{c}1,\tan {\mathrm{\&theta;}}_3\&GreaterEqual;d_4/a_3\\ 0,\tan {\mathrm{\&theta;}}_3<d_4/a_3\end{array}\right.,{\mathrm{\&theta;}}_3\&Element;(-\mathrm{\&pi;},\mathrm{\&pi;})---\left(4\right)$

$\mathrm{cfg}3=\left\{\begin{array}{c}1,{\mathrm{\&theta;}}_5<0\\ 0,{\mathrm{\&theta;}}_5\&GreaterEqual;0\end{array}\right.,{\mathrm{\&theta;}}_3\&Element;(-2\mathrm{\&pi;},2\mathrm{\&pi;})---\left(5\right)$

接着在下一步中，例如可以采用反变换法（也称代数法）执行运动学反解。具体而言，已知机器人末端执行机构的空间位姿如表达式（1）和（2）所示，则根据以下表达式分别依次求出关于机器人各个关节变量θ₁~θ₆的唯一正确值：

θ₁=Atan2(p_y,p_x)                          （6）

此时θ₁有两个解，但根据预先设定的关节特征属性，假设cfg1=1时，则应当满足c₁p_x+s₁p_y≥0，因此可以将所求出的两个解分别代入，并取满足c₁p_x+s₁p_y≥0的解，相应可获得关于关节变量θ₁的唯一正确值，同时满足机器人能够在避开奇异形位的情况下实现正常位姿的要求。

下面利用已求得的关于关节变量θ₁的唯一正确值，继续求解关节变量θ₃：

${\mathrm{\&theta;}}_3=A\tan 2(k,\&PlusMinus;\sqrt{{a_3}^2+{d_4}^2-k^2})-A\tan 2(a_3,d_4)$

$k=\frac{{(c_1{p}_x+s_1{p}_y-a_1)}^2+{p_z}^2-{a_2}^2-{a_3}^2-{d_4}^2}{2a_2}---\left(7\right)$

此时θ₃同样有两个解，但根据预先设定的关节特征属性，假设cfg2=0，则应当满足tanθ₃<d₄/a₃，因此可以将所求出的两个解分别代入，并取满足此条件的解，相应可获得关于关节变量θ₃的唯一正确值。

在求出关于关节变量θ₁、θ₃的唯一正确值之后，可以依照下列表达式直接求出关于关节变量θ₂的唯一解。

θ₂=Atan2[(a₃+a₂c₃)p_z+(c₁p_x+s₁p_y-a₁)(a₂s₃+d₄),-(d₄+a₂s₃)p_z+(c₁p_x+s₁p_y-a₁)(a₂c₃+a₃)]-θ₃           （8）

在求出关节变量θ₁~θ₃的唯一正确值之后，继续求解关节变量θ₅：

${\mathrm{\&theta;}}_5=A\tan (\&PlusMinus;\sqrt{{(a_x{c}_1{c}_{23}+a_y{s}_1{c}_{23}+a_z{s}_{23})}^2+{(a_x{s}_1-a_y{c}_1)}^2},a_x{c}_1{s}_{23}+a_y{s}_1{s}_{23}-a_z{c}_{23})---\left(9\right)$

此时θ₅同样有两个解，但根据预先设定的关节特征属性，假设cfg3=1，则应当满足θ₅<0，因此可以根据此条件，相应可获得关于关节变量θ₅的唯一正确值。

最后，可以根据已经求出的θ₁~θ₃、θ₅值以及下列表达式（10）和（11），分别计算出关于θ₄和θ₆的唯一解：

${\mathrm{\&theta;}}_4=\left\{\begin{array}{c}A\tan (a_x{s}_1-a_y{c}_1,a_x{c}_1{c}_{23}+a_y{s}_1{c}_{23}+a_z{s}_{23}),{\mathrm{\&theta;}}_5>0\\ A\tan (a_y{c}_1-a_x{s}_1,-a_x{c}_1{c}_{23}-a_y{s}_1{c}_{23}-a_z{s}_{23}),{\mathrm{\&theta;}}_5<0\end{array}\right.---\left(10\right)$

${\mathrm{\&theta;}}_6=\left\{\begin{array}{c}A\tan (o_x{c}_1{s}_{23}+o_y{s}_1{s}_{23}-o_z{c}_{23},-n_x{c}_1{s}_{23}-n_y{s}_1{s}_{23}+n_z{c}_{23}),{\mathrm{\&theta;}}_5>0\\ A\tan (-o_x{c}_1{s}_{23}-o_y{s}_1{s}_{23}+o_z{c}_{23},n_x{c}_1{s}_{23}+n_y{s}_1{s}_{23}-n_z{c}_{23}),{\mathrm{\&theta;}}_5<0\end{array}\right.---\left(11\right)$

上述表达式（6）~(11)即为六自由度工业机器人的各个关节变量的反解公式，根据这些公式以及所设定的关节特征属性，相应可计算出各个关节变量的唯一解。相应地，根据所求出的各个关节变量的唯一解的组合，可执行对六自由度工业机器人的关节运动，由此完成符合需求的运动控制过程。

为了更清楚地解释和说明本发明，下面以莫托曼SK6型机器人为例，分别按照正反解过程来执行运动学算法，由此进一步验证按照本发明的运动控制方法。

首先是运动学正解过程，也即已知机器人各个关节的关节变量及各连杆参数等固有参数值，求出依照这些关节变量所能实现的机器人末端执行机构的位置和姿态最终结果。假定已经给定各关节变量θ₁~θ₆依次为30°、60°、30°、90°、30°和45°，并且经查表可知该型号机器人的a₁~a₃分别为150mm、450mm和115mm，d₄为735mm，因此按照上述的表达式（1）和（2），可直接计算出该机器人的末端执行机构相对于基坐标系的位姿矩阵

为：

$T_6^0=\left[\begin{array}{cccc}0& 0& 1& 555\sqrt{3}\\ -\frac{\sqrt{2}}{2}& \frac{\sqrt{2}}{2}& 0& 555\\ \frac{\sqrt{2}}{2}& -\frac{\sqrt{2}}{2}& 0& 115+225\sqrt{3}\\ 0& 0& 0& 1\end{array}\right]$

下面再假设已知的条件仅为机器人末端执行机构相对于基坐标系的位姿矩阵（也即仅知机器人末端执行机构最终所处的位置和姿态、以及连杆参数等固有参数），通过执行按照本发明的运动学反解过程来求出机器人的各关节变量θ₁~θ₆的唯一正确值。相应地，可验证这些值是否与正解过程相符合。

根据上述表达式（6），可计算出该弧度值换算成角度值即等于30°或-150°。然而，由于根据需要已经设定机器人的关节特征属性组合应为110，也即cfg1=1、cfg2=1且cfg3=3，在此情况下只有当θ₁=30°时才能满足cfg1=1的条件，因此此时取θ₁=30°为满足条件的唯一解。

同理，可以根据已求出的θ₁值以及表达式（7），计算出 ${\mathrm{\&theta;}}_3=A\tan 2(k,\sqrt{{a_3}^2+{d_4}^2-k^2})-A\tan 2(a_3,d_4)=0.523599,$ 换算成角度即30°或-150°。然而，由于已知cfg2=1并且只有当θ₃=30°时cfg2=1，因此此时取θ₃=30°为满足条件的唯一解。接着，根据已求出的θ₁值和θ₃值以及表达式（8），可求出关于θ₂的唯一解，即θ₂=60°。

同理，可以根据已经求出的θ₁~θ₃值以及表达式（9），可计算出 ${\mathrm{\&theta;}}_5=A\tan (\sqrt{{(a_x{c}_1{c}_{23}+a_y{s}_1{c}_{23}+a_z{s}_{23})}^2+{(a_x{s}_1-a_y{c}_1)}^2},a_x{c}_1{s}_{23}+a_y{s}_1{s}_{23}-a_z{c}_{23})=0.523599,$ 换算成角度即30°或-150°。然而，由于已知cfg3=0并且只有当θ₅=30°时cfg3=0，因此此时取θ₅=30°为满足条件的唯一解。

最后，可以根据已经求出的θ₁~θ₃、θ₅值以及表达式（10）和（11），分别计算出关于θ₄和θ₆的唯一解，即θ₄=90°，θ₆=45°。

将正反解过程的已知条件及求解结果相对照可见，按照本发明引入关节特征属性之后，机器人的运动学反解过程不仅可省去复杂的比较择优过程，提高运动学反解速度，还能预知机器人运动中的过奇异点路径和无解路径。例如，若路径起止示教点的“前/后”关节属性不同，机器人在做直角坐标系下的运动时可能会经过属性临界点，也就是位置奇异点，而要使机器人做线性运动的同时变更关节属性是不可能的。所以在进行机器人路径规划时，如果路径起止点处于不同的关节属性状态下，则不允许机器人做线性或圆弧运动，而只能做关节运动。因此，与现有方法相比，本发明的六自由度工业机器人运动控制方法具备运算效率高、易于编程实现、可实现对唯一解的正确反解过程等优点，因此对于六自由度机器人的精确运动控制方面具备重要的现实意义。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种解耦型六自由度工业机器人的运动控制方法，该六自由度工业机器人包括底座、腰部回转部件、大臂、小臂、手腕部件和末端执行机构，并通过第一关节完成机器人腰部的回转运动，通过第二、第三关节分别执行大臂和小臂的俯仰动作，以及通过第四至第六关节共同执行末端执行机构的位姿操作，所述方法包括下列步骤：

（a）根据对机器人末端执行机构所需实现的位置和姿态，通过D-H模型法获得末端执行机构相对于基坐标系的位姿矩阵

$T_6^0=T_1^{0}T_2^{1}T_3^{2}T_4^{3}T_5^{4}T_6^5=\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]---\left(1\right)$

该位姿矩阵中各个元素分别如以下表达式组（2）所示，其中c₁~c₆、s₁~s₆分别依次表示机器人各个关节变量θ₁~θ₆的余弦值和正弦值，c₂₃、s₂₃分别表示关节变量θ₂与θ₃之和的余弦值和正弦值，a₁~a₃分别表示机器人第一、第二和第三关节各自与其相邻的下一关节之间的轴线距离，d₄表示第四关节与末端执行机构之间的距离：第三关节轴线和第四、五关节轴线交点之间的距离：

$\left.\begin{array}{c}{n}_x=c_1[c_{23}(c_4{c}_5{c}_6-s_4{s}_6)-s_{23}{s}_5{c}_6]+s_1(s_4{c}_5{c}_6+c_4{s}_6)\\ n_y=s_1[c_{23}(c_4{c}_5{c}_6-s_4{s}_6)-s_{23}{s}_5{c}_6]-c_1(s_4{c}_5{c}_6+c_4{s}_6)\\ n_z=s_{23}(c_4{c}_5{c}_6-s_4{s}_6)+c_{23}{s}_5{c}_6\\ o_x=c_1[c_{23}(-c_4{c}_5{c}_6-s_4{c}_6)+s_{23}{s}_5{s}_6]+s_1(c_4{c}_6-s_4{c}_5{s}_6)\\ o_y=s_1[c_{23}(-c_4{c}_5{c}_6-s_4{c}_6)+s_{23}{s}_5{s}_6]-c_1(c_4{c}_6-s_4{c}_5{s}_6)\\ o_z=s_{23}(-c_4{c}_5{c}_6-s_4{c}_6)-c_{23}{s}_5{s}_6\\ a_x=c_1(c_{23}{c}_4{s}_5+s_{23}{c}_5)+s_1{s}_4{s}_5\\ a_y=s_1(c_{23}{c}_4{s}_5+s_{23}{c}_5)-c_1{s}_4{s}_5\\ a_z=s_{23}{c}_4{s}_5-c_{23}{c}_5\\ p_x=c_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ p_y=s_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ p_z=a_2{s}_2+a_3{s}_{23}-d_4{c}_{23}\end{array}\right\}---\left(2\right)$

（b）将机器人避开奇异形位时所能实现的正常位姿定义为不同的关节特性属性cfg1、cfg2和cfg3，并设定机器人末端执行机构实现所需的位姿时的关节特征属性组合，其中所述关节特性属性所代表的意义和取值条件分别如下：

cfg1表示当机器人手腕部件的中心点分别处于第一参考平面左右两侧时的状态，并且满足 $\mathrm{cfg}1=\left\{\begin{array}{c}1,c_1{p}_x+s_1{p}_y\&GreaterEqual;0\\ 0,c_1{p}_x+s_1{p}_y<0\end{array}\right.,$ 所述第一参考平面是以机器人第一关节的转动轴线与第二关节的轴线平行线两者所构成的平面；

cfg2表示机器人手腕部件的中心点分别处于第二参考平面左右两侧时的状态，并且满足 $\mathrm{cfg}2=\left\{\begin{array}{c}1,\tan {\mathrm{\&theta;}}_3\&GreaterEqual;d_4/a_3\\ 0,\tan {\mathrm{\&theta;}}_3<d_4/a_3\end{array}\right.,{\mathrm{\&theta;}}_3\&Element;(-\mathrm{\&pi;},\mathrm{\&pi;}),$ 所述第二参考平面是以机器人大臂和第三关节的轴线所构成的平面；

cfg3表示机器人手腕部件的中心点分别处于第三参考平面左右两侧时的状态，并且满足 $\mathrm{cfg}3=\left\{\begin{array}{c}1,{\mathrm{\&theta;}}_5<0\\ 0,{\mathrm{\&theta;}}_5\&GreaterEqual;0\end{array}\right.,{\mathrm{\&theta;}}_3\&Element;(-2\mathrm{\&pi;},2\mathrm{\&pi;}),$ 所述第三参考平面是以机器人小臂和第五关节的轴向所构成的平面；

（c）根据步骤（a）所获得位姿矩阵、以及步骤（b）所设定的关节特征属性组合及其取值条件，通过反变换法分别求得关于机器人各个关节变量θ₁~θ₆的唯一解；

（d）根据所求得的各个关节变量的解，执行对六自由度工业机器人的关节运动，相应完成所需位姿的运动控制过程。

2.如权利要求1所述的运动控制方法，其特征在于，所述反变换法求解机器人各个关节变量的过程具体包括以下步骤：

（c1）根据表达式θ₁=Atan2(p_y,p_x)，求出关于关节变量θ₁的两个解，并依照所设定的所述关节特征参数cfg1，从这两个解中获得关于关节变量θ₁的唯一解；

（c2）利用步骤（c1）所求得的关于关节变量θ₁的唯一解，并根据以下表达式组求出关于关节变量θ₃的两个解，并依照所设定的所述关节特征参数cfg2，从这两个解中获得关于关节变量θ₃的唯一解：

${\mathrm{\&theta;}}_3=A\tan 2(k,\&PlusMinus;\sqrt{{a_3}^2+{d_4}^2-k^2})-A\tan 2(a_3,d_4)$

$k=\frac{{(c_1{p}_x+s_1{p}_y-a_1)}^2+{p_z}^2-{a_2}^2-{a_3}^2-{d_4}^2}{2a_2}$

（c3）利用所求得的关于关节变量θ₁、θ₃的唯一解，根据以下表达式直接求出关于关节变量θ₂的唯一解：

θ₂=Atan2[(a₃+a₂c₃)p_z+(c₁p_x+s₁p_y-a₁)(a₂s₃+d₄),-(d₄+a₂s₃)p_z+(c₁p_x+s₁p_y-a₁)(a₂c₃+a₃)]-θ₃

（c4）利用所求得的关于关节变量θ₁~θ₃的唯一解，根据以下表达式求出关于关节变量θ₅的两个解，同时依照所设定的所述关节特征参数cfg3，从这两个解中获得关于关节变量θ₅的唯一解；

（c5）利用所求得的关于关节变量θ₁~θ₃、θ₅的唯一解，根据以下表达式分别求出关于关节变量θ₄和θ₆的唯一解：

${\mathrm{\&theta;}}_4=\left\{\begin{array}{c}A\tan (a_x{s}_1-a_y{c}_1,a_x{c}_1{c}_{23}+a_y{s}_1{c}_{23}+a_z{s}_{23}),{\mathrm{\&theta;}}_5>0\\ A\tan (a_y{c}_1-a_x{s}_1,-a_x{c}_1{c}_{23}-a_y{s}_1{c}_{23}-a_z{s}_{23}),{\mathrm{\&theta;}}_5<0\end{array}\right.$

${\mathrm{\&theta;}}_6=\left\{\begin{array}{c}A\tan (o_x{c}_1{s}_{23}+o_y{s}_1{s}_{23}-o_z{c}_{23},-n_x{c}_1{s}_{23}-n_y{s}_1{s}_{23}+n_z{c}_{23}),{\mathrm{\&theta;}}_5>0\\ A\tan (-o_x{c}_1{s}_{23}-o_y{s}_1{s}_{23}+o_z{c}_{23},n_x{c}_1{s}_{23}+n_y{s}_1{s}_{23}-n_z{c}_{23}),{\mathrm{\&theta;}}_5<0\end{array}\right..$

Images