一种六关节工业机器人通过姿态奇点的控制方法及系统

摘要

本发明属于工业机器人运动控制领域，并具体公开了一种六关节工业机器人通过姿态奇点的控制方法及系统，包括如下步骤：1)截取姿态奇点所处工业机器人预设的规划路径中的一段轨迹，获取轨迹起点的位置值和后三个关节变量值以及终点的位置值和后三个关节变量值；2)以插补方式获得姿态奇点位置的后三个关节变量值；3)求取姿态奇点位置对应的前三个关节变量值；4)以所述姿态奇点位置对应的后三个关节变量值以及前三个关节变量值作为工业机器人在姿态奇点位置对应的运动控制参数，实现工业机器人的运动控制，以此使得六关节工业机器人顺利通过姿态奇点。本发明在不改变原有规划路径的前提下使得机器人顺利通过姿态奇点，简单易行且运算效率高。

说明书

技术领域

本发明属于工业机器人运动控制领域，更具体地，涉及一种六关节工业机器人通过姿态奇点的控制方法及系统。

背景技术

运动控制指令是实现以指定速度、特定路线模式等将工具从一个位置移动到另一个指定位置，在使用运动指令时需指定采用什么运动方式来控制到达指定位置的运动路径，机器人运动的类型有三种：关节运动(J)、直线运动(L)、圆弧运动(C)。六关节工业机器人的后三个关节轴线交于一点，当机器人第四关节和第六关节轴线重合，运动相互抵消，机器人将丧失一个自由度，此为机器人俯/仰关节属性的临界点，也是机器人的内部奇异形位，可称为机器人的姿态奇点。当处于姿态奇点时，机器人的运动类型为直线运动或者圆弧运动，此时运动控制器不知该如何选取第四关节和第六关节的值，导致机械臂失控并发出警报，使机器人无法按照规划的路径通过姿态奇点，这种情况在实际应用中经常出现，不仅使机器人的运动空间缩小，而且给生产带来了很大的不便。

目前对于工业机器人的姿态奇点主要有以下三种处理方法：1)避让机器人的姿态奇点：当遇到姿态奇点时，改变机器人的运动路径从而避让奇异形位；2)路径规划方法：采用对姿态奇点进行路径规划的方法使得机器人通过姿态奇点，通过迭代运算得到关节变量值的解析解；3)调整姿态法：当处于姿态奇点时，第四关节和第六关节的关节变量的和或差为定值，通过协调两者的值使得机器人通过姿态奇点。

然而进一步的研究表明，上述方法仍然存在以下问题：避让姿态奇点法虽然使机器人从姿态奇点附近的路径通过，避开了姿态奇点，但是其改变了机器人原有的路径并且具有盲目性，需要对机器人所有的姿态奇点进行避让，可操作性不强，同时也使得机器人的空间范围缩小；路径规划方法是对姿态奇点所处的路径重新进行规划，采用复杂的迭代运算求得机器人关节变量的解析解，不能得到精确的数值解，使得机器人不能准确的通过姿态奇点，并且计算繁琐，工作量较大；调整姿态法虽然可以使得机器人准确的通过姿态奇点，但是在协调运动的过程中机器人处于停留状态，使得机器人的效率降低，并且可能导致协调后的运动速度不连续，给实际生产带来不便。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种六关节工业机器人通过姿态奇点的控制方法及系统，其采用插补替换机器人末端姿态的方式，在不改变原有规划路径的前提下使得机器人顺利通过姿态奇点，由此解决目前因姿态奇点导致机器人运动范围受限的技术问题，具有简单易行、运算效率高、适用性强等特点。

为实现上述目的，按照本发明的一个方面，提出了一种六关节工业机器人通过姿态奇点的控制方法，其包括如下步骤：

(1)截取姿态奇点所处工业机器人预设的规划路径中的一段轨迹，获取该轨迹起点对应的位置值和后三个关节变量值以及该轨迹终点对应的位置值和后三个关节变量值；

(2)根据所述起点的后三个关节变量值以及终点的后三个关节变量值以插补的方式获得姿态奇点位置对应的后三个关节变量值；

(3)建立运动学反解方程求取姿态奇点位置对应的前三个关节变量值；

(4)以所述姿态奇点位置对应的后三个关节变量值以及前三个关节变量值作为工业机器人在姿态奇点位置对应的运动控制参数，实现工业机器人的运动控制，以此使得六关节工业机器人顺利通过姿态奇点。

作为进一步优选的，所述步骤(3)包括如下子步骤：

(3.1)首先建立如下矩阵方程：

其中，x_t,y_t,z_t为工具坐标系原点相对于基座标系的三个坐标分量，s₁＝sin(θ₁)，s₂＝sin(θ₂)，s₂₃＝sin(θ₂+θ₃)，c₁＝cos(θ₁)，c₂＝cos(θ₂)，c₂₃＝cos(θ₂+θ₃)，θ₁,θ₂,θ₃为工业机器人第一关节至第三关节的关节变量，a₁和a₂分别表示第一连杆和第二连杆的长度，d₂表示第一连杆和第二连杆之间的偏置，x₃,y₃,z₃为工具坐标系原点相对于第三连杆坐标系的三个坐标分量；

(3.2)对步骤(3.1)中建立的矩阵方程两端同时左乘第一连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵，建立运动学反解方程，求得第一关节和第三关节的关节值θ₁和θ₃：

其中，a₁和a₂分别表示第一连杆和第二连杆的长度，d₂表示第一连杆、第二连杆之间的偏置；

(3.3)对步骤(3.1)中建立的矩阵方程两端同时左乘第三连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵，建立运动学反解方程，求得第二关节的关节值θ₂：

其中，c₃＝cos(θ₃)，s₃＝sin(θ₃)。

作为进一步优选的，所述插补的总点数N优选采用下述方式确定：

其中：x₁,y₁,z₁为轨迹起点的位置值，x₂,y₂,z₂为轨迹终点的位置值，V为插补速度，T为插补周期。

作为进一步优选的，所述插补速度优选为10mm/s，插补周期优选为1ms。

作为进一步优选的，所述步骤(1)之前还包括如下步骤：首先根据六关节工业机器人建立连杆坐标系，并对预设的规划路径进行运动学反解获得工业机器人的各关节变量值，然后判断六关节工业机器人是否处于姿态奇点，若是，则进入步骤(1)，若否，则以反解获得的工业机器人的各关节变量值实现工业机器人的运动控制。

按照本发明的另一个方面，提供了一种六关节工业机器人通过姿态奇点的控制系统，其包括：

数据获取模块，用于截取姿态奇点所处工业机器人预设的规划路径中的一段轨迹，获取该轨迹起点对应的位置值和后三个关节变量值以及该轨迹终点对应的位置值和后三个关节变量值；

插补模块，用于根据所述起点的后三个关节变量值以及终点的后三个关节变量值以插补的方式获得姿态奇点位置对应的后三个关节变量值；

数据处理模块，用于建立运动学反解方程并求取姿态奇点位置对应的前三个关节变量值；

运动控制模块，用于以所述姿态奇点位置对应的后三个关节变量值以及前三个关节变量值作为工业机器人在姿态奇点位置对应的运动控制参数，实现工业机器人的运动控制，以此使得六关节工业机器人顺利通过姿态奇点。

作为进一步优选的，所述数据处理模块包括如下子模块：

矩阵方程建立子模块，用于建立如下方程：

其中，x_t,y_t,z_t为工具坐标系原点相对于基座标系的三个坐标分量，s₁＝sin(θ₁)，s₂＝sin(θ₂)，s₂₃＝sin(θ₂+θ₃)，c₁＝cos(θ₁)，c₂＝cos(θ₂)，c₂₃＝cos(θ₂+θ₃)，θ₁,θ₂,θ₃为工业机器人第一关节至第三关节的关节变量，a₁和a₂分别表示第一连杆和第二连杆的长度，d₂表示第一连杆和第二连杆之间的偏置，x₃,y₃,z₃为工具坐标系原点相对于第三连杆坐标系的三个坐标分量；

第一关节值计算子模块，用于对矩阵方程建立子模块建立的矩阵方程两端同时左乘第一连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵，建立运动学反解方程，求得第一关节和第三关节的关节值θ₁和θ₃：

第二关节值计算子模块，用于对矩阵方程建立子模块建立的矩阵方程两端同时左乘第三连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵，建立运动学反解方程，求得第二关节的关节值θ₂：

其中，c₃＝cos(θ₃)，s₃＝sin(θ₃)。

作为进一步优选的，所述插补模块进行插补时的插补总点数N优选采用下述方式确定：

其中：x₁,y₁,z₁为轨迹起点的位置值，x₂,y₂,z₂为轨迹终点的位置值，V为插补速度，T为插补周期。

作为进一步优选的，所述插补速度优选为10mm/s，插补周期优选为1ms。

作为进一步优选的，所述控制系统还包括判断模块，用于根据六关节工业机器人建立连杆坐标系，并对预设的规划路径进行运动学反解获得工业机器人的各关节变量值，然后判断六关节工业机器人是否处于姿态奇点，若是，则进入步骤(1)，若否，则以反解获得的工业机器人的各关节变量值实现工业机器人的运动控制。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：

1.本发明采用将工业机器人末端原有位置保持不变，插补得到工业机器人末端姿态的方式，在不改变原有规划路径的前提下使得工业机器人顺利通过姿态奇点，大大降低了通过外部设备渡过姿态奇点的生产成本，避免了采用路径规划方式得到解析解的复杂迭代运算，可直接得到关节位置的数值解，简单易行且运算效率高，适用于后三个关节交于一点的工业机器人。

2.本发明从工业机器人运动控制领域出发，使得工业机器人顺利通过姿态奇点，扩展了工业机器人的运动控制指令范围，使得工业机器人的运动空间得到扩大，更加的灵活和智能化，避免了因姿态奇点造成运动范围缩小或报警等问题，可直接将程序植入到工业机器人运动控制器中，使得工业机器人不用停留即可顺利通过姿态奇点，给实际生产带来很大便利。

3.本发明还可以应用到无姿态奇点的路径规划，不通过运动学反解得到工作路径中一系列点的关节值，进而避免了反解过程中第四关节的关节变量值无法求取的现象，可直接通过插补得到关节值，简单易行，实用性强。

附图说明

图1是六关节工业机器人通过姿态奇点的流程图；

图2(a)和(b)是华中数控6012工业机器人的构型图；

图3是华中数控6012工业机器人的各连杆坐标系。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明的一种六关节工业机器人通过姿态奇点的控制方法，具体包括如下步骤：

(1)截取姿态奇点所处工业机器人预设的规划路径中的一段轨迹，获取该轨迹起始点的位置值和后三个关节变量值以及轨迹终点的位置值和后三个关节变量值；

(2)根据所述起始点的后三个关节变量值以及终点的后三个关节变量值以插补的方式获得姿态奇点位置对应的后三个关节变量值；

(3)求取姿态奇点位置对应的前三个关节变量值；

(4)以所述姿态奇点位置对应的后三个关节变量值以及前三个关节变量值作为工业机器人在姿态奇点位置对应的运动控制参数，实现工业机器人的运动控制，以此使得六关节工业机器人顺利通过姿态奇点。

为了便于各个变量的求取与计算，在步骤(1)之前需做如下准备工作：根据六关节工业机器人(包括六个关节和六根连杆，分别为第一关节、第二关节、第三关节、第四关节、第五关节、第六关节，第一连杆、第二连杆、第三连杆、第四连杆、第五连杆、第六连杆)的结构特点建立连杆坐标系，该坐标系可采用诸多现有方式进行建立，本发明以D-H法建立坐标系为例进行示例性说明，不作为对本发明的限定。

根据工业机器人的结构特点利用D-H法建立连杆坐标系：将关节i的轴线方向作为坐标系{i}的z轴z_i，将关节i和i+1的轴线公垂线方向作为坐标系{i}的x轴x_i，指向由关节i到关节i+1，根据右手法则规定坐标系{i}的y轴y_i，x_i和y_i的交点作为坐标系{i}的原点o_i，基坐标系任意选定，为了简单方便起见，优选的基坐标系{0}与连杆1的坐标系{1}重合，将工业机器人后三个关节轴线的交点选作为连杆坐标系{4}，{5}和{6}的原点，至此建立工业机器人的各连杆坐标系。

根据上述所设定的连杆坐标系，相应的连杆参数可定义如下：

a_i-1＝从z_i-1到z_i沿x_i-1测量的距离；

α_i-1＝从z_i-1到z_i绕x_i-1旋转的角度；

d_i＝从x_i-1到x_i沿z_i测量的距离；

θ_i＝从x_i-1到x_i绕z_i旋转的角度。

根据建立的各连杆坐标系将相应的连杆参数列出，利用所列的连杆参数计算出各个连杆变换矩阵推导出第三连杆坐标系相对于基坐标系的等价其次变换矩阵和第六连杆坐标系相对于第三连杆坐标系的齐次变换矩阵

其中，i＝0,1...,6，θ₁,θ₂,θ₃为第一关节至第三关节的关节变量，{i}表示与连杆i固接的坐标系,为工业机器人连杆坐标系{i}相对于{i+1}的齐次变换矩阵。

具体的，如图3所示，第一关节的轴线为铅直方向，将其作为坐标系{1}的z轴z₁，优选的指向向上，将第一关节和第二关节的轴线公垂线方向作为坐标系{1}的x轴x₁，指向由第一关节到第二关节，那么x₁和z₁的交点即为坐标系{1}的原点o₁；第二关节和第三关节的轴线为水平方向，分别作为坐标系{2}和{3}的z轴z₂和z₃，且z₂和z₃相互平行，将第二关节和第三关节的轴线公垂线方向作为坐标系{2}的x轴x₂，x₂和z₂的交点即为坐标系{2}的原点o₂；将第三关节和第四关节的轴线公垂线方向作为坐标系{3}的x轴x₃，x₃和z₃的交点即为坐标系{3}的原点o₃；基坐标系可以任意选定，为了简单方便起见，优选的基坐标系{0}与第一连杆的坐标系{1}重合；第四关节,第五关节和第六关节的轴线交于一点，将该点选作为连杆坐标系{4}，{5}和{6}的原点o_4,5,6，将第四关节的轴线方向作为坐标系{4}的z轴z₄，优选的坐标系{4}的x轴x₄的方向与x₃一致，将第五关节的轴线方向作为坐标系{5}的z轴z₅，优选的坐标系{6}的z轴z₆的方向与z₄一致，优选的坐标系{5}的x轴x₅和坐标系{6}的x轴x₆的方向与x₄一致；优选的工具末端中心作为工具坐标系{T}的原点o_T，优选的工具坐标系{T}的x轴x_T的方向与x₆一致，优选的工具坐标系{T}的z轴z_T的方向与z₆一致。从z₁到z₂沿x₁的距离为a₁，从x₁到x₂沿z₂的距离为d₂，从z₂到z₃沿x₂的距离为a₂，从z₃到z₄沿x₃的距离为a₃，从x₃到x₄沿z₄的距离为d₄。根据建立的各连杆坐标系将相应的连杆参数列出，利用所列的连杆参数计算出各个连杆变换矩阵推导出第三连杆坐标系相对于基坐标系的等价其次变换矩阵和第六连杆坐标系相对于第三连杆坐标系的齐次变换矩阵

其中，i＝0,1...,6，θ_i为关节i的关节变量，s_i＝sin(θ_i)，c_i＝cos(θ_i)，s₂₃＝sin(θ₂+θ₃)，c₂₃＝cos(θ₂+θ₃)，{i}表示与连杆i固接的坐标系,为工业机器人连杆坐标系{i}相对于{i-1}的齐次变换矩阵。

建立连杆坐标系后，对预设的规划路径(该路径由实际运动需求确定，本发明不做限定)进行运动学反解获得工业机器人的各关节变量值，其为现有技术，在此不赘述，正常情况下工业机器人根据预设的规划路径进行运动，也即不处于姿态奇点时，工业机器人正常运行，当第四关节的关节变量值无法求取，即第五关节的关节变量为零时，此时工业机器人处于姿态奇点，由于第五关节变量为零，难以顺利通过，因此本发明对工业机器人的运动位姿进行以下处理：

(1)截取姿态奇点所处规划路径中的一段轨迹，为了提高运算效率，优选的轨迹为姿态奇点所在的整个圆弧段或者直线段，然后获取此轨迹起始点的位置值和后三个关节变量值，以及轨迹终点的位置值和后三个关节变量值；

(2)然后将起始点的位置值和后三个关节变量值，以及轨迹终点的位置值和后三个关节变量值输送到插补器中进行插补获得姿态奇点位置对应的后三个关节变量值，具体的根据起始和终点位置值，插补速度V和插补周期T可得到要插补的总点数N，插补器的插补速度和插补周期可根据工业机器人的实际需求给定，考虑到插补的精度和效率，优选的插补速度为10mm/s，插补周期为1ms，根据插补总点数N将轨迹起点和轨迹终点之间插补成N份，也即轨迹起点和轨迹终点之间的后三个关节变量值之间也插补成N份，得到姿态奇点位置对应的后三个关节变量值；其中，轨迹起点、轨迹终点的和插补得到的姿态奇点的位置值和后三个关节变量值分别为：x₁,y₁,z₁,θ₄₁,θ₅₁,θ₆₁,x₂,y₂,z₂,θ₄₂,θ₅₂,θ₆₂,x_t,y_t,z_t,θ_4t,θ_5t,θ_6t，

当处于姿态奇点时，工业机器人的运动类型为直线运动或者圆弧运动，因此通过插补器插补得到的各点均在原有规划路径上。

(3)求取姿态奇点位置对应的前三个关节变量值

(3.1)首先建立如下矩阵方程：

其中，x_t,y_t,z_t为工具坐标系原点相对于基坐标系的三个坐标分量，s₁＝sin(θ₁)，s₂＝sin(θ₂)，s₂₃＝sin(θ₂+θ₃)，c₁＝cos(θ₁)，c₂＝cos(θ₂)，c₂₃＝cos(θ₂+θ₃)，θ₁,θ₂,θ₃为工业机器人第一关节至第三关节的关节变量，a₁和a₂分别表示第一连杆和第二连杆的长度，d₂表示第一连杆和第二连杆之间的偏置，x₃,y₃,z₃为工具坐标系原点相对于第三连杆坐标系的三个坐标分量；

具体的采用如下方式建立矩阵方程：

将后三个连杆变换矩阵和工具坐标系相对于第六连杆坐标系的齐次变换矩阵相乘可以得到：

即

其中，表示工具坐标系相对于第三连杆坐标系的旋转矩阵，表示工具坐标系相对于第六连杆坐标系的旋转矩阵，表示工具坐标系相对于第三连杆坐标系的位置矢量，表示工具坐标系相对于第六连杆坐标系的位置矢量；

令等式两端的第四列元素对应相等，姿态奇点的后三个关节变量值已经通过插补获得，等式的右边均为已知：

将工业机器人的前三个关节进行固连(即无相对运动)，已知工具坐标系相对于基座标系的位置矢量为：工具坐标系相对于第三连杆坐标系的位置矢量为：后三个关节变量已通过插补获得，将工业机器人的运动学方程写成：

即

令上述矩阵方程两端的第四列元素对应相等，可建立如下方程：

即

(3.2)对步骤(3.1)中建立的矩阵方程两端同时左乘第一连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵，建立运动学反解方程，求得第一关节和第三关节的关节值θ₁和θ₃：

具体为：在步骤(3.1)中建立的矩阵方程两端同时左乘第一连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵建立运动学反解方程如下：

化解得：

式中，可由求逆获得。

令矩阵方程(1)两端第二行第一列的元素对应相等得：

-s₁x_t+c₁y_t＝-z₃-d₂；

若则得到第一关节的关节变量值θ₁：

选定θ₁其中的一个解后，令矩阵方程(1)两端第一行第四列和第三行第四列的元素分别对应相等得：

上述两式的平方和为：

-s₃y₃+c₃x₃＝k，

若则得到第三关节的关节变量值θ₃：

(3.3)对步骤(3.1)中建立的矩阵方程两端同时左乘第三连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵，建立运动学反解方程，求得第二关节的关节值θ₂：

其中，c₃＝cos(θ₃)，s₃＝sin(θ₃)。

具体为：在步骤(3.1)中建立的矩阵方程两端同时左乘第三连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵建立运动学反解方程如下：

化解得

式中，可由求逆获得。

令矩阵方程(2)两端第一行第四列和第二行第四列的元素分别对应相等得：

将上面两个方程联立求解得到s₂₃和：

可以看出s₂₃和c₂₃表达式的分母相等，且为正，于是求得第二关节的关节变量值θ₂：

其中，s₂＝sin(θ₂)；c₂＝cos(θ₂)；s₂₃＝sin(θ₂+θ₃)；c₂₃＝cos(θ₂+θ₃)。

(4)以所述姿态奇点位置对应的后三个关节变量值以及前三个关节变量值作为工业机器人在姿态奇点位置对应的运动控制参数，实现工业机器人的运动控制，以此使得六关节工业机器人顺利通过姿态奇点。也即将由上述算法得到的前三个关节变量值和插补得到的后三个关节变量值传送到工业机器人运动控制器中，使得工业机器人的六个关节在姿态奇点均具有确定数值，以此方式，实现工业机器人顺利通过姿态奇点。

本发明还提供了一种六关节工业机器人通过姿态奇点的控制系统，包括：数据获取模块，用于截取姿态奇点所处工业机器人预设的规划路径中的一段轨迹，获取该轨迹起点对应的位置值和后三个关节变量值以及该轨迹终点对应的位置值和后三个关节变量值；插补模块，用于根据所述起点的后三个关节变量值以及终点的后三个关节变量值以插补的方式获得姿态奇点位置对应的后三个关节变量值；数据处理模块，用于建立运动学反解方程并求取姿态奇点位置对应的前三个关节变量值；运动控制模块，用于以所述姿态奇点位置对应的后三个关节变量值以及前三个关节变量值作为工业机器人在姿态奇点位置对应的运动控制参数，实现工业机器人的运动控制，以此使得六关节工业机器人顺利通过姿态奇点。

具体的，所述数据处理模块包括如下子模块：

矩阵方程建立子模块，用于建立如下方程：

其中，x_t,y_t,z_t分别为工具坐标系原点相对于基座标系的三个坐标分量，s₁＝sin(θ₁)，s₂＝sin(θ₂)，s₂₃＝sin(θ₂+θ₃)，c₁＝cos(θ₁)，c₂＝cos(θ₂)，c₂₃＝cos(θ₂+θ₃)，θ₁,θ₂,θ₃为工业机器人第一关节至第三关节的关节变量，a₁和a₂分别表示第一连杆和第二连杆的长度，d₂表示第一连杆和第二连杆之间的偏置，x₃,y₃,z₃分别为工具坐标系原点相对于第三连杆坐标系的三个坐标分量；

第一关节值计算子模块，用于对矩阵方程建立子模块建立的矩阵方程两端同时左乘第一连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵，建立运动学反解方程，求得第一关节和第三关节的关节值θ₁和θ₃：

第二关节值计算子模块，用于对矩阵方程建立子模块建立的矩阵方程两端同时左乘第三连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵，建立运动学反解方程，求得第二关节的关节值θ₂：

其中，c₃＝cos(θ₃)，s₃＝sin(θ₃)。

具体的，所述插补模块进行插补时的插补总点数N优选采用下述方式确定：

其中：x₁,y₁,z₁为轨迹起点的位置值，x₂,y₂,z₂为轨迹终点的位置值，V为插补速度，T为插补周期。所述插补速度优选为10mm/s，插补周期优选为1ms。

具体的，所述控制系统还包括判断模块，用于根据六关节工业机器人建立连杆坐标系，并对预设的规划路径进行运动学反解获得工业机器人的各关节变量值，然后判断六关节工业机器人是否处于姿态奇点，若是，则进入步骤(1)，若否，则以反解获得的工业机器人的各关节变量值实现工业机器人的运动控制。

以下为具体实施例：

本实施例以华中数控6012工业机器人为例，其构型如图2(a)和(b)所示，华中数控6012工业机器人属于关节式机器人，其由6个连杆(如图2所示的第一连杆1、第二连杆2、第三连杆3、第四连杆4、第五连杆5、第六连杆6)和6个转动关节(如图3所示的点A、B、C、D、E、F，)组成，基座固定不动称为连杆0，第一关节连接第一连杆和第二连杆，第二关节连接第二连杆和第三连杆，以此类推，工具与连杆6固接。其通过姿态奇点的过程主要包括以下步骤：

步骤1)：根据华中数控6012工业机器人的结构特点和D-H法，设定的连杆坐标系{0}，{1}，{2}，{3}，{4}，{5}，{6}和{T}如图3所示：基坐标系{0}与连杆1的坐标系{1}重合，坐标系{1}的z轴z₁为第一关节的轴线方向，指向向上，坐标系{1}的x轴x₁为第一关节和2的轴线公垂线方向，指向由第一关节到第二关节，那么坐标系{1}的原点o₁即为x₁和z₁的交点；至此坐标系{1}建立完成，以此类推建立坐标{2}和{3}；连杆坐标系{4}，{5}和{6}的原点o_4,5,6为第四关节,5和6的轴线交点，坐标系{4}的z轴z₄为第四关节的轴线方向，坐标系{4}的x轴x₄的方向与x₃一致，坐标系{5}的z轴z₅为第五关节的轴线方向，坐标系{6}的z轴z₆的方向与z₄一致，坐标系{5}的x轴x₅和坐标系{6}的x轴x₆的方向与x₄一致；工具坐标系{T}的原点o_T为工具末端中心，工具坐标系{T}的x轴x_T的方向与x₆一致，工具坐标系{T}的z轴z_T的方向与z₆一致。

根据建立的各连杆坐标系将相应的连杆参数列出，如表1所示。

表1华中数控6012工业机器人的连杆参数

利用所列的连杆参数计算出各个连杆变换矩阵，推导出第三连杆坐标系相对于基坐标系的等价其次变换矩阵

步骤2)：工业机器人处于姿态奇点时的位置为：(1323.1,226.8,703.8)，此姿态奇点所在的路径为直线，截取其所处规划路径中的整个直线段，此直线段起点的位置值为(999.2,450.9,532.3)，后三个关节变量的关节值为(-108.5°,-30.6°,223.8°),终点的位置值为(1512.3,105.6,796.4)，后三个关节变量的关节值为(36.3°,-15.1°,77.1°)。

步骤3)：将起始点、终点的位置和后三个关节变量值均输送到插补器中，插补速度为10mm/s，插补周期为1ms，插补的总点数N：

将轨迹起点和轨迹终点的后三个关节变量值之间也插补成68份，得到姿态奇点位置对应的后三个关节变量值：(-11.9°,-20.2°,119.3°)。

步骤4)：已知工具坐标系相对于第六连杆坐标系的位置矢量为：将通过插补得到的后三个关节变量值θ₄，θ₅，θ₆代入式(2)得：

故工具坐标系相对于第三连杆坐标系的位置矢量为：

将工业机器人的前三个关节进行固连，在姿态奇点，已知工具坐标系相对于基座标系的位置矢量为：后三个关节变量θ₄，θ₅，θ₆已通过插补获得，将工业机器人的运动学方程写成：

令矩阵方程(4)两端的第四列元素对应相等，可建立如下方程：

矩阵方程(5)两端同时左乘第一连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵建立运动学反解方程如下：

令矩阵方程(6)两端第二行第一列的元素对应相等得：-1332.1s₁+226.8c₁＝-346.8(7)；解得第一关节的关节变量值：θ₁＝174.8°或θ₁＝24.5°。

选定θ₁其中的一个解后，令矩阵方程(6)两端第一行第四列和第三行第四列的元素分别对应相等得：

上述两式的平方和为：-(-1254)s₃+(-42.1)c₃＝k(9)；

其中，

解得当θ₁＝174.8°时第三关节的关节变量值：θ₃＝-343.5°或θ₃＝-192.6°；当θ₁＝24.5°时第三关节的关节变量值：θ₃＝-9.8°或θ₃＝-166.4°。

矩阵方程(5)两边同时左乘第三连杆坐标系相对于基座标系的齐次变换矩阵的逆矩阵建立运动学反解方程如下：

令矩阵方程(10)两端第一行第四列和第二行第四列的元素分别对应相等得：

将上面两个方程联立求解得到s₂₃和c₂₃，

解得：θ₂₃＝-141°，θ₂₃＝-86.1°，θ₂₃＝88.8°，θ₂₃＝158.5°。

故第二关节的关节变量值为：θ₂＝202.5°，θ₂＝106.5°，θ₂＝98.6°，θ₂＝324,9°。前三个关节变量反解一共有四组解，如表2所示：

表2华中数控6012工业机器人前三个关节变量的反解值

θ₁θ₂θ₃第一组174.8°202.5°-343.5°第二组174.8°106.5°-192.6°第三组24.5°98.6°-9.8°第四组24.5°324.9°-166.4°

只有第三组解符合表1给定的关节的运动范围，即前三个关节变量分别为：θ₁＝24.5°，θ₂＝98.6°，θ₃＝-9.8°。

步骤5)：将由上述算法得到的前三个关节变量值和插补得到的后三个关节变量值传送到工业机器人运动控制器中，使得工业机器人的六个关节在姿态奇点均具有确定数值；以此方式，实现工业机器人顺利通过姿态奇点。

本实例根据上述位置及运动学反解方程，求得机器人位于姿态奇点时的前三个关节变量，并通过插补得到后三个关节的关节变量，经过运动学正解验算得到的机器人末端位置与实际末端位置一致。

本发明提供了一种六关节工业机器人通过姿态奇点的控制方法，在不改变原有规划路径的前提下使得机器人顺利通过姿态奇点，大大降低了通过外部设备渡过姿态奇点的生产成本，避免了路径规划方式到解析解的复杂迭代运算，可直接得到机器人处于姿态奇点时关节位置的数值解，简单易行且运算效率高，此外本发明实现了机器人运动控制指令范围的扩展，可直接将程序植入到机器人运动控制器中，给实际生产带来很大便利。

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