一种离线编程中的复杂曲面加工轨迹生成方法与流程

本发明属于智能制造和工业机器人离线编程领域，特别涉及一种离线编程中的复杂曲面加工轨迹生成方法。

背景技术：

在实际工业制造过程中，工业机器人因其通用性强、灵活度高、占地面积小和工作范围大的特点在越来越来越多应用场景下代替人工，离线编程技术也飞速发展，代替了低效率、低精度、耗时长传统示教编程，但是在许多复杂场景下，离线编程技术还受到限制。像工业生产中常见的叶轮机叶片研磨抛光、汽车表层喷涂以及一些工艺品制造的雕刻都涉及到复杂曲面的加工，而这类加工轨迹主要通过示教编程的方式获得，不仅效率低下，复杂度高，精度低，而且浪费了大量的人力成本。对于复杂曲面加工这类高精度的生产需求，往往需要高精度的复杂曲面轨迹生成技术，这类技术可以通过加工对象的三维模型提取出相应的几何信息与拓扑信息，并且根据实际生产需求，利用相关的曲面轨迹生成算法自动地生成机器人的加工路径，可以很好地满足这种复杂且精度高的加工任务。

技术实现要素：

为了克服现有离线编程技术在复杂曲面加工需求下的不足，本发明的目的在于提供一种离线编程中的复杂曲面加工轨迹生成方法，实现多种工业场景下高精度的复杂曲面轨迹加工。

为了实现上述目的，本发明采用的技术方案是：

一种离线编程中的复杂曲面加工轨迹生成方法，包括：

步骤1：利用复杂曲面轨迹生成算法初步生成机器人加工轨迹；

步骤2：对步骤1中生成的机器人加工轨迹进行检测，找出机器人运动学中的异常点；

步骤3：对步骤2中找出的异常点进行修正；

步骤4：利用多线程技术进行对异常点修正后的加工轨迹在离线编程软件中进行虚拟仿真；

步骤5：仿真成功后进行后置代码转换到真实环境中运行。

所述步骤1中，利用基于step标准的nurbs复杂曲面轨迹生成算法初步生成机器人加工轨迹，包括如下子步骤：步骤1.1：获取待加工曲面，其属于基于step标准的nurbs曲面；步骤1.2：提取所述待加工曲面的几何信息及拓扑信息；步骤1.3：定义约束面、原点坐标、法向正方向、参数范围以及初始平移的间距，采用基于曲率的截面法生成待加工曲面所有轨迹点的位置坐标；步骤1.4：对轨迹点进行插补，得到轨迹的姿态信息，用欧拉角表示；步骤1.5：按照加工次序连接所有生成的轨迹点，并设置轨迹点的运动模式，将第一个轨迹点的运动模式设计为关节空间运动“movj”，其余轨迹点的运动模式设置为笛卡尔空间直线运动“movl”。

所述步骤1.2中，先提取实体信息，然后再提取几何信息和拓扑信息，其中实体信息的提取步骤如下：步骤1.2.1：读取step模型文件中一行的字符串；步骤1.2.2：判断是否含有字符“#”，如有则至步骤1.2.3，否则执行步骤1.2.5；步骤1.2.3：采用字符串分割算法分割当前字符串，每行的字符串均进行分割，分割的具体步骤如下：

步骤1.2.3.1：首先提取“#”和“＝”之间的数字作为实体序号；步骤1.2.3.2：提取“＝”和“(”间的字符串作为实体的类型；步骤1.2.3.3：提取“(”和第一个“,”之间的字符串作为实体名称；步骤1.2.3.4：提取“(”和“)”中第一个“,”之后的通过”,”分的字符串，依次作为实体的参数属性；步骤1.2.4：保存实体信息到结构体；步骤1.2.5：判断下一行字符串是否为空，如果为空，读取文件结束，反之回到步骤1.2.1，直至提取完成step模型文件中的数据；

几何信息的提取步骤如下：

步骤1.2.6：首先定位到setp模型文件中用来表示带节点的b样条曲面的实体b_spline_surface_with_knots；步骤1.2.7：根据nurbs曲面数学模型依次提取参数；步骤1.2.8：依次定位到控制点列表中每个实体cartesian_point；步骤1.2.9：提取控制点实体中的三维坐标信息，即几何信息；

拓扑信息的提取步骤：

步骤1.2.10：首先定位到step模型文件壳实体，提取壳参数；步骤1.2.11：定位到组成壳的高级面，提取曲面参数；步骤1.2.12：定位到组成高级面的面边界和几何曲面，提取面边界参数；步骤1.2.13：定位到组成面的边界环，提取环参数；步骤1.2.14：定位到组成环的边，提取边参数；步骤1.2.15：定位到边的首尾端点，提取端点参数；由此得到拓扑信息；之后，根据step模型文件中的拓扑信息初始化opencascade中对应的拓扑对象，结合几何信息提取过程中生成的opencascade几何对象，将曲面信息全部转化为opencascade的实体对象。

所述步骤1.3中，基于曲率的截面线法是用一组互相平行的约束面与待加工曲面相交，然后对相交线进行离散化，具体步骤如下：

步骤1.3.1：首先定义一个初始化的约束面，约束面的法向方向即为进给方向；步骤1.3.2：沿进给方向平移约束面，直到约束面与待加工曲面相交；步骤1.3.3：对相交线进行离散，并与曲面内外环求交得到有效曲线段；步骤1.3.4：对有效曲线采用自由曲线插补算法，针对插补点采用行距计算模型计算在允许残余高度范围下最大约束面间距；步骤1.3.5：选择插补点中最小约束面间距，平移约束面得到第二条与待加工曲面相交的相交线，即沿法向平移约束面行距的距离，若与待加工曲面相交，返回步骤1.3.3，不相交，至步骤1.3.6；步骤1.3.6：结束。

所述步骤1.3.4对有效曲线采用自由曲线插补算法，具体包括如下步骤：

步骤1.3.4.1：首先设定允许的最大弦高误差δ允；步骤1.3.4.2：连接当前参数区间左右边界对应的曲线段的首尾端点pfirst和plast做一条直线，并且计算该直线段与曲线段的误差，判断是否小于δ允，若不满足转至步骤1.3.4.3，否则执行步骤1.3.4.4；步骤1.3.4.3：缩短曲线的参数区间，范围由[umin,umax]变为返回步骤1.3.4.2；umin,umax对应参数区间内曲线首尾端的参数值；步骤1.3.4.4：通过曲线参数方程计算参数区间右边界utemp相应的点ptemp，判断是否是曲线终止点，若不是终止点，则执行步骤1.3.4.5，否则插补结束；步骤1.3.4.5：将当前参数区间右边界的参数utemp作为曲线参数空间的左边界，umax作为参数空间的右边界，即参数空间更新为[utemp,umax]，继续执行步骤步骤1.3.4.2；

所述行距计算模型包括平面加工行距计算模型、凸曲面加工行距计算模型和凹曲面加工行距计算模型；

所述步骤1.3.5中，首先定义c1和c2是两个相邻的约束面，p1和p2是两个相邻轨迹点，θ是约束平面法向量与向量p1p2的夹角，由几何关系计算得到空间上约束面的平移距离d为：d＝l·cosθ。

所述步骤1.4中，采用欧拉角来描述轨迹点的姿态信息，机器人的运动轨迹参数有六个，前三个是轨迹点的x、y、z坐标值，用来描述末端位置，后三个是欧拉角α、β、γ，用来描述末端姿态，在轨迹生成中，利用右手笛卡尔坐标系定义轨迹点的姿态，定义坐标系的原点为当前轨迹点，坐标系的x轴dirx为轨迹点所在曲线的切向量，坐标系的z轴dirz为轨迹点在待加工曲面上的法向量，默认法向量朝向曲面外部，最后通过x轴和z轴叉乘得到y轴diry＝dirz×dirx，机器人在加工时，工具末端坐标系的x轴方向与轨迹点x轴方向保持一致，工具末端保证在工件外部，因此工具末端坐标系的z轴方向与轨迹点z轴方向相反，工具末端坐标系的x、z、y轴正方向分别为：dirx、-dirz、diry＝(-dirz)×dirx。

所述步骤2中，所述异常点包括奇异点、轴超限点、不可达点以及跳变点，将所生成的轨迹点按照位姿和机器人机械臂的运动范围进行运动学分析，找出奇异点、不可达点、轴超限点及跳变点，具体步骤如下：

步骤2.1：选取需要进行检测的曲面轨迹，提取轨迹信息，并按轨迹加工顺序将轨迹点按顺序存储，获取轨迹点的总个数，设置运动指令计数标志，初始化为0；

步骤2.2：如果计数标志小于轨迹点总个数，获取当前轨迹点，提取轨迹点信息，反之，进入步骤2.6；

步骤2.3：解析运动指令，通过步骤2.2得到轨迹点信息，通过机器人逆解算法得到在该轨迹点下的运动关节角，如果逆解无解或jacobian矩阵无反函数，则该点不可达，或该点为奇异点，将轨迹点设为不可达点或奇异点，运动指令计数标志加一，返回步骤2.2；如果逆解有解，解出六个关节角并保存，到步骤2.4；

步骤2.4：由步骤2.3得知当前轨迹点不是不可达点或奇异点。将逆解求出的六个关节角进行轴超限检测，如果有任意一轴的关节角超出范围，将该轨迹点的轴超限状态设置为激活状态，如果该点是正常点，到步骤2.5；

步骤2.5：根据运动方式，对该点进行插补，在插补过程中，得到一个插补点，如果该点不是最后一个插补点，返回步骤2.2；如果该点是最后一个插补点，将计数标志加1，返回步骤2.2；

步骤2.6：结束，此时检测出该复杂曲面轨迹加工过程中，所有遇到的不可达点及轴超限点，并保存了所有轨迹点及其插补点的关节角；

步骤2.7：进行跳变点检测，具体步骤如下：

步骤2.7.1：首先设置六个关节角的变化量分别为a1，a2，a3，a4，a5，a6；初始化为0，然后得到包括轨迹点和插补点在内的所有点的个数，设置计数器，初始化为0，设置前一步的六个关节角的值pre1～pre6，初始化为第一个点六个关节角的值；

步骤2.7.2：通过计数器从所有点中按顺序得到一个轨迹点，得到这个点的六个关节角cur1～cur6；

步骤2.7.3：通过公式：得到当前轨迹点关节角的变化量，如果关节角变化量大于50度则将该轨迹点中的跳变标志位激活，表面该轨迹点是一个跳变点；

步骤2.7.4：判断计数器是否小于轨迹点的总数，是则返回步骤2.7.2，反之，结束；

通过以上方法将曲面每个轨迹点的状态数据检测更新，将轨迹点分为五类：正常点、不可达点或奇异点、轴超限点、跳变点。

所述步骤3中，对检测出来的异常点进行位姿调整，实现对于单轨迹点、轨迹段、多轨迹点的多种编辑的功能，主要编辑操作包括旋转、复制、删除、平移和轴反向，用来对异常轨迹点进行操作，以求变为正常可以加工的轨迹点；

对于一段轨迹，除了对单轨迹点的操作外，还包括位姿的统一、隐藏、显示、仿真、轨迹反向、关联零件、上下移动；

在具体实现中，利用计算机中基本的数据结构链表来存储轨迹点。轨迹的复制，平移、反向、上下移动等功能利用链表的特性来实现；隐藏、显示是可视化的需求，利用occ封装的函数来实现，以方便用户操作。

所述步骤4中，将生成的轨迹通过关联零件操作与加工零件相互关联，借助occ中gp_trsf描述模型对象位姿变换，具体的步骤如下：

步骤1.4.1：首先获取当前轨迹所在工件模型的变化gp_trsf；

步骤1.4.2：获取轨迹中所有点的位置坐标，构造对应的gp_pnt，并且得到读取所有点的姿态信息，即三个欧拉角，根据欧拉角构造对应的三个方向gp_dir；

步骤1.4.3：根据步骤1.4.1得到的工件的变换矩阵gp_trsf，将该轨迹中所有点的gp_pnt和三个方向gp_dir变换到相应的位置；

步骤1.4.4：根据每个点的gp_dir，计算该轨迹中所有点变换后的欧拉角；

步骤1.4.5：更新轨迹数据，并同时更新交互界面中的显示模型。

所述步骤4中，对于两机器人异步协同加工的仿真，单独设置一类通信轨迹点来通过多线程并发的方式来完成，并通过线程之间的通信来完成机器人之间的通信以完成相互协作的任务，具体步骤如下：

步骤1.4.6：读取两台机器人所有的轨迹信息，分别存储在相应的轨迹列表中；

步骤1.4.7：分别对两个机器人的轨迹点列表根据位姿信息，运动模式进行运动学解析，得到每个轨迹点与插补点的关节角和变换矩阵；

步骤1.4.8：根据步骤1.4.7得到的变换矩阵和关节角，通过_beginthread函数并发两个线程，并保存两个线程的句柄；同时利用变换矩阵对两台机器人的各个关节的位姿进行更新，利用occ显示在视图中；

步骤1.4.9：对于一个线程执行如下操作：遍历轨迹点，读取一个轨迹点，判断该轨迹点是否为通信轨迹点，如果是，唤起兄弟线程即另一个机器人仿真线程，然后挂起自己，当整个轨迹遍历完后，唤醒一次兄弟线程，防止发生锁死。

与现有技术相比，本发明可适用广泛的复杂曲面加工轨迹的生成，利用基于step模型的复杂曲面轨迹生成技术和基于工业机器人运动学原理的轨迹点检测及编辑技术，利用多线程方式来满足两台机器人异步协同加工复杂曲面仿真的要求，可以很大程度上节省离线编程中复杂曲面加工轨迹的规划时间，提高编程效率，进一步提高工业机器人的智能化程度。

附图说明

图1是本发明复杂曲面加工轨迹生成总体流程图。

图2是伸缩步长法原理示意图。

图3是行距与约束面间距转化示意图。

图4是截面线法示意图。

图5是平面加工行距计算示意图。

图6是凸曲面加工行距计算示意图。

图7是凹曲面加工行距计算示意图。

图8是复杂曲面轨迹生成算法流程图。

图9是曲面拓扑信息拓扑信息提取流程图。

图10是轨迹编辑器模块结构图。

图11是采用外部旋转的zyx旋转变换示意图。

图12是本发明实体分层次结构图。

具体实施方式

下面结合附图和实施例详细说明本发明的实施方式。

本发明在实现过程中，使用了一种开源的计算机图形学库(几何造型平台)opencascade(occ)来实现一些基础的模型操作，几何运算，矩阵运算等辅助功能。

如图1所示，本发明一种离线编程中的复杂曲面加工轨迹生成方法，主要步骤包括：

步骤1：利用复杂曲面轨迹生成算法初步生成机器人加工轨迹；

步骤2：对步骤1中生成的机器人加工轨迹进行检测，找出机器人运动学中的异常点，具体可根据运动学正逆解等方法实现；

步骤3：对步骤2中找出的异常点进行修正，并可进一步提供一种可视化的交互式修正功能；

步骤4：利用多线程技术进行对异常点修正后的加工轨迹在离线编程软件中进行虚拟仿真；

步骤5：仿真成功后进行后置代码转换到真实环境中运行。

下面分别对各步骤进行详细解释。

步骤1中，利用了基于step标准的nurbs复杂曲面轨迹生成算法初步生成机器人加工轨迹，即根据step模型文件，生成拾取到面的轨迹点的位置，姿态及运动方式，参考图8，具体包括如下子步骤：

步骤1.1：获取待加工曲面，显然，其属于基于step标准的nurbs曲面；

步骤1.2：从step模型文件数据段开始读取数据，通过字符串的形式加载到计算机内存中，然后进行字符串分割，提取实体信息，具体步骤如下：

步骤1.2.1：读取step模型文件中一行的字符串；

步骤1.2.2：判断是否含有字符“#”，如有则至步骤1.2.3，否则执行步骤1.2.5；

步骤1.2.3：采用字符串分割算法分割当前字符串，每行的字符串均进行分割，分割的具体步骤如下：

步骤1.2.3.1：首先提取“#”和“＝”之间的数字作为实体序号；

步骤1.2.3.2：提取“＝”和“(”间的字符串作为实体的类型；

步骤1.2.3.3：提取“(”和第一个“,”之间的字符串作为实体名称；

步骤1.2.3.4：提取“(”和“)”中第一个“,”之后的通过”,”分的字符串，依次作为实体的参数属性；

步骤1.2.4：保存实体信息到结构体；

步骤1.2.5：判断下一行字符串是否为空，如果为空，读取文件结束，反之回到步骤1.2.1，直至提取完成step模型文件中的数据；

在提取完成step模型文件中的数据后，可进行几何信息及拓扑信息提取，step模型文件中通过参数曲面描述曲面的几何信息(包括解析曲面)，曲面统一使用nurbs曲面进行描述，根据stepap203协议中拓扑实体表示，由高到低的层次排列基础的拓扑实体为：壳(shell)、面(face)、边界环(loop)、边(edge)和顶点(vertex)，拓扑信息是描述的是实体所包含的点、边、环等元素之间的层次关系，一个壳由面组成，一个面由内外环组成，一个环由边组成，一个边由顶点组成，拓扑信息将基础拓扑实体有序的组织在一起，即准确描述一个实体，几何信息提取的具体步骤如下：

步骤1.2.6：首先定位到step模型文件中用来表示带节点的b样条曲面的实体b_spline_surface_with_knots；

其中，b_spline_surface_with_knots是step标准里的b样条曲线的表示；

步骤1.2.7：根据nurbs曲面数学模型依次提取参数；

步骤1.2.8：依次定位到控制点列表中每个实体cartesian_point，cartesian_point是step模型文件中曲面信息；

步骤1.2.9：提取控制点实体中的三维坐标信息，即几何信息；

参考图9，step模型文件中曲面拓扑信息根据拓扑实体的约束关系，按照从高到低的顺序进行提取，即按照壳、面、环、边、点的顺序依次提取，即拓扑信息的提取步骤如下：

步骤1.2.10：首先定位到step模型文件壳实体，提取壳参数；

步骤1.2.11：定位到组成壳的高级面，提取曲面参数；

步骤1.2.12：定位到组成高级面的面边界和几何曲面，提取面边界参数；

步骤1.2.13：定位到组成面的边界环，提取环参数；

步骤1.2.14：定位到组成环的边，提取边参数；

步骤1.2.15：定位到边的首尾端点，提取端点参数；

即，在提取拓扑信息时，要按照壳、面、环、边、点的顺序依次提取。

最后根据step模型文件中的拓扑信息初始化opencascade中对应的拓扑对象，结合几何信息提取过程中生成的opencascade几何对象，将曲面信息全部转化为opencascade的实体对象。

本发明中，基础拓扑实体的定义说明如下：

(1)壳(shell)：最常见的一种壳是封闭壳，壳由一系列高级面封闭而成。壳的具体数据格式如下：：

#28＝closed_shell(”,(#8,#38,#54,#89,#158,#952,#1212,#1278)；

28表示该封闭壳的实体序号，#8，#38，#54，#89，#158，#952，#1212，#1278表示构成这个封闭壳的10个高级面。

(2)面(face)：主要分为面曲面(face_surface)、子面(subface)、有向面(oriented_face)。其中用的最多是面曲面的子类高级面(advanced_face)，高级面是由面的边界(face_bound)和几何曲面实体定义，用布尔值来表示该面的法向是否与定义它的几何曲面一致。一个面实体的数据格式如下：

#45＝advanced_face(”,(#255),#345,.f.)；

#255＝face_bound(”,#560,.f.)；

#345＝plane(”,#204)；

58表示该面的实体序号，#255表示定义面的面边界，#354表示定义面的几何曲面，且该拓扑面的法向与定义它的几何平面的法向不一致。

(3)环(loop)：环的子类包括顶点环(vertex_loop)、边环(edge_loop)和多边形环(poly_loop)。其具体的数据表示方法如下：

#29＝edge_loop(”,(#34,#56,#65,#84,#138))；

其中29表示该环的实体序号，#34,#56,#65,#84,#138表示定义这个环的5条首尾相连的有向边。

(4)边(edge)：边的子类包括有向边(oriented_edge)和边曲线(edge_curve)。有向边中的布尔标识表示有向边的方向与边曲线的方向是否保持一致。其具体的数据表示方法如下：

#25＝oriented_edge(”,*,*,#48,.t.)；

#48＝edge_curve(”,#69,#121,#203,.t.)；

#69＝vertex_point(”,#130)；

#121＝vertex_point(”,#142)；

#203＝surface_curve(”,#224,(#428,#382),.pcurve_s2.)；

25表示该边的实体序号，#48定义这个边的边曲线。#69和#121表示定义边曲线#48的首尾端点，#203定义了对应的几何曲线，t.表示边#25的方向与边曲线#48的方向一致。

(5)顶点(vettex)：最常用的是顶点点(vertex_point)。具体在step中的数据形式如下：

#22＝vertex_point(”,#43)；

#43＝cartesian_point(”,(-25.29321342079,-2.076297072243,2.93633))；

22表示这个点的实体序号，#43表示定义该点三维坐标值的笛卡尔点#43。

关于步骤1.2中信息提取的说明：

本发明在离线编程软件中模型导入采用的是基于step标准的cad模型文件，在进行加工轨迹生成前需要获取待加工曲面的几何信息和拓扑信息，信息提取的过程分为三个部分：读取step模型文件、几何信息提取和拓扑信息提取。step模型文件中是通过参数曲面描述曲面几何信息的，包括解析曲面也表示为参数曲面，曲面统一使用nurbs曲面进行描述。常见的几何实体包括点(point)、方向(direction)、几何曲线(curve)、几何曲面(surface)和体(manifold_solid_brep)。本发明主要研究的是几何曲面实体，几何曲面有初等曲面(elementary_surface)、曲线有界曲面(curve_bounded_surface)、扫掠曲面(swept_surface)和b样条曲面(b_spline_surface)等。step模型文件中拓扑信息通过stepap203协议中拓扑实体表示。根据从高到低的层次排列基础的拓扑实体有壳(shell)、面(face)、边界环(loop)、边(edge)和顶点(vertex)，拓扑信息描述的是实体所包含的点、边、环等元素之间的层次关系，一个壳由面组成，一个面由内外环组成，一个环由边组成，一个边由顶点组成，拓扑信息将基础拓扑实体有序的组织在一起，就可以准确描述一个实体。两个几何信息完全相同的模型，如果拓扑信息不一样，所构造的实体也完全不同，因此几何信息和拓扑信息的结合，才能完整描述一个空间三维实体的位置和形状。拓扑实体可以和几何实体相互关联，顶点对应点，边对应曲线，面对应曲面。

即，本发明中，实体分为几何实体和拓扑实体，层次结构图如图12所示，图中矩形表示几何实体，圆角矩形表示拓扑实体。

步骤1.3：定义约束面、原点坐标、法向正方向、参数范围以及初始平移的间距，采用基于曲率的截面线法生成待加工曲面所有轨迹点的位置坐标；

复杂曲面加工轨迹生成主要是机器人的末端执行器位姿，包括空间位置信息(x，y，z空间坐标)和姿态信息，因此基于曲率的截面线法是用一组互相平行的约束面与待加工曲面相交，然后对相交线进行离散化，参考图4，具体步骤如下：

步骤1.3.1：首先定义一个初始化的约束面，约束面的法向方向即为进给方向；

步骤1.3.2：沿进给方向平移约束面，直到约束面与待加工曲面相交；

步骤1.3.3：然后对相交线进行离散，并与曲面内外环求交得到有效曲线段，见图4；

步骤1.3.4：对有效曲线采用自由曲线插补算法，针对插补点采用行距计算模型计算在允许残余高度范围下最大约束面间距；

对生成的有效曲线进行插补具体可采用基于弦高误差插补法中的伸缩步长法，在自由曲线插补过程中，能够根据加工精度的需求设定允许的弦高误差的最大值，在插补时，能够保证相邻的插补点之间连接的短直线与相应的曲线段之间的弦高误差不大于设定的最大值，相比于等参数插补法更适合自由曲线，其原理如图2所示，具体步骤如下：

步骤1.3.4.1：首先设定允许的最大弦高误差δ允；

步骤1.3.4.2：连接当前参数区间左右边界对应的曲线段的首尾端点pfirst和plast,做一条直线，并且计算该直线段与曲线段的误差，判断是否小于允许的最大弦高误差δ允，若不满足转至步骤1.3.4.3，否则执行步骤1.3.4.4；

步骤1.3.4.3：缩短曲线的参数区间，范围由[umin,umax]变为返回步骤1.3.4.2；其中根据向量函数的概念，空间中一条曲线可以表示为参数u的向量函数，定义如下：

c(u)＝(x(u),y(u),z(u)),u∈[a,b]

式中：(x(u),y(u),z(u))表示该参数空间一点的坐标；a和b表示曲线的首尾端点参数，由此参数空间定义为[umin,umax]，即umin和umax限制了曲线首尾端的参数值。

步骤1.3.4.4：通过曲线参数方程计算参数区间右边界utemp相应的点ptemp，判断是否是曲线终止点，若不是终止点，则执行步骤1.3.4.5，否则插补结束。

步骤1.3.4.5：将当前参数区间右边界的参数utemp作为曲线参数空间的左边界，umax作为参数空间的右边界，即参数空间更新为[utemp,umax]，继续执行步骤步骤1.3.4.2；

通过步骤1.3.4.1～步骤1.3.4.5，保证相邻插补点之间连接的直线与相应曲线之间的弦高误差一定不会大于设定的允许误差，保证曲线插补点满足实际加工的精度要求。

步骤1.3.5：选择插补点中最小约束面间距平移约束面得到第二条与曲面相交的相交线，即沿法向平移约束面行距的距离，若与曲面相交，返回步骤1.3.3，不相交，至步骤1.3.6；

步骤1.3.6：结束。

本发明中nurbs曲线的定义及数学表达：

k次nurbs曲线的定义：

其中：u是曲线参数；pi(i＝0,1,…,n)表示n+1个控制顶点；bi,k(u)是b样条基函数；ωi为控制顶点pi的权因子，ω0,ωn＞0，其余ωi≥0。

b样条基函数bi,k(u)定义为：

其中：u是曲线参数；ti表示第i个节点；t＝[t0,t1,…,tn+k+1]表示b样条曲线的参数节点矢量。

节点矢量中单调非降序排列节点，即ti≤ti+1，i＝0,1,…,n+k。在实际应用中，节点矢量t＝[t0,t1,…,tn+k+1]通常两端按多重节点取值，定义为：

本发明中，截面线法是利用一系列相互平行的约束面与待加工曲面相交得到截面线，然后生成轨迹的方法，在opencascade中，平面由一个平面原点和法向矢量唯一定义得到，因此可以利用opencascade为用户提供自定义约束面的功能，使用者只需要在几何视图中选取平面原点和平面法向即可完成约束面的定义；其中，平面原点可以通过手动输入原点x、y、z坐标值，也可通过拾取虚拟场景中点实体然后得到相应的空间坐标值；平面法向的定义是以平面原点为起点，以输入的平面法向坐标值为终点，生成的矢量作为约束面法向的正方向，平面法向坐标值同样支持手动输入和拾取点实体更新坐标两种方式。

在截面线法中需要根据当前曲率动态更新进给方向行距，行距一定程度上决定了曲面的产品精度和生产效率，需要一个行距计算模型，分为平面加工行距计算模型、凸曲面加工行距计算模型和凹曲面加工行距计算模型；

在加工平面时，加工行距计算图如图5所示，允许的最大残余高度h和行距l的关系为：

其中：rtool为工具的半径；

在实际应用中，rtool远大于h，因此上式可以简化为：

凸曲面加工行距的计算示意图如图6所示，首先需要确定机器人加工末端的边缘圆弧所在方程定义：

其中：θ为两个接触点之间的角度值；q＝rtool+rsurface，rsurface是凸曲面沿约束面平移方向的局部近似曲率半径；

由几何关系计算得到两个接触点之间的角度值的三角函数值：

将得到的三角函数带入机器人加工末端边缘圆弧方程求得交点m的坐标：

再通过得到的交点坐标计算出残余高度h的值：

最后根据上面的计算得到最后需要的行距l：

在实际应用中，rtool远大于h，因此上式可以简化为：

最后是凹曲面加工行距计算模型，在加工凹曲面时，加工行距计算示意图如图7所示，与凸曲面的计算步骤相似，最后得到凹曲面的行距计算公式如下：

由上述关系计算得到平面、凸曲面、凹曲面三种加工情况下的行距计算模型，需要沿约束面法向平移约束面，接下来可以根据行距求解得到约束面的平移间距d。首先定义c1和c2是两个相邻的约束面，p1和p2是两个相邻轨迹点，θ是约束平面法向量与向量p1p2的夹角，可以由几何关系计算得到空间上约束面的平移距离d为：d＝l·cosθ，如图3所示。

在有了上述的插补算法和加工行距计算模型后，按照步骤生成复杂曲面轨迹，首先定义一个初始化的约束面，约束面的法向方向即为进给方向；然后沿进给方向平移约束面，直到约束面与待加工曲面相交；对相交线进行离散，并与曲面内外环交得到有效曲线段；最后对有效曲线采用自由曲线插补算法，针对插补点采用行距计算模型计算在允许残余高度范围下最大约束面间距。

步骤1.4：对轨迹点进行插补，得到轨迹的姿态信息，用欧拉角表示。

在曲面轨迹生成模块中，不仅需要得到机器人运动的轨迹点三维坐标数据，还需要得到轨迹点的姿态信息，姿态信息决定了机器人的工具末端到达空间点的姿态，与实际加工效果关系紧密。本方法采用欧拉角来描述轨迹点的姿态信息，机器人的运动轨迹参数有六个，前三个是轨迹点的x、y、z坐标值，用来描述末端位置，后三个是欧拉角α、β、γ，用来描述末端姿态，在轨迹生成中，一般利用局部坐标系定义末端姿态；

在occ中，一个几何曲面(geom_surface)与一个拓扑面(topods_face)相关联，通过截面线法得到的一系列几何曲线(geom_curve)与拓扑边(topods_edge)相关联，通常情况下拓扑边包含于拓扑面中，而相交得到的一系列拓扑边都在待加工曲面上。轨迹点的姿态可以通过坐标系进行描述，本发明利用右手笛卡尔坐标系定义姿态。定义该坐标系的原点为当前轨迹点，坐标系的x轴dirx为轨迹点所在曲线的切向量，坐标系的z轴dirz为轨迹点在待加工曲面上的法向量，此处默认法向量朝向曲面外部，最后通过x轴和z轴叉乘得到y轴diry＝dirz×dirx。通常情况下，机器人在加工时，工具末端坐标系的x轴方向与轨迹点x轴方向保持一致，工具末端必须保证在工件外部，因此工具末端坐标系的z轴方向与轨迹点z轴方向相反，工具末端坐标系的x、z、y轴正方向分别为：dirx、-dirz、diry＝(-dirz)×dirx；

occ中geom2dlprop包和geomlprop包提供了关于几何模型局部属性的算法，采用occ中关于局部属性的算法可以得到工具末端坐标系的x、y、z轴方向向量，从而得到相应的旋转矩阵，两个机器人分别通过各自欧拉角计算公式得到旋转矩阵对应的欧拉角；

具体地，对于不同的机器人，轨迹姿态信息生成的方法不相同，本发明提供一种基于图灵机器人和航天赛能机器人的曲面轨迹姿态生成方法，对于图灵机器人，采用外部zyx方式进行旋转得到变换矩阵，再通过该变化矩阵求出对应的三个欧拉角值，用以表示轨迹点的姿态；对于航天赛能机器人，采用内在旋转的zyx方式得到变换矩阵，每一次变换需要用上一次变化后得到的变换矩阵来计算新的变换矩阵，再通过最后的变换阵求出对应的三个欧拉角值，用以表示轨迹点的姿态。

以图灵机器人为例：

相对于固定坐标系旋转的zyx方式，所参考的固定坐标系为机器人基坐标系，假设最初机器人末端坐标系与机器人基坐标系完全重合，末端坐标系先绕基坐标系的z轴旋转γ弧度，然后绕基坐标系y轴旋转β弧度，再绕x轴旋转α弧度，最后得到当前机器人末端坐标系，旋转弧度的正负由右手系判断得到。设待旋转的坐标系为x'y'z'，固定参照坐标系为xyz，如图11所示为外部旋转的zyx旋转变换，其中环形箭头指向绕坐标轴的旋转方向。

绕基坐标系z轴、y轴、x轴分别旋转γ弧度、β弧度、α弧度的旋转矩阵rz(γ)、ry(β)、rx(α)为：

采用外部旋转zyx方式的旋转矩阵rzyx可以表示为：

rzyx＝rx(α)·ry(β)·rz(γ)

根据旋转矩阵，可以得到图灵机器人相应的三个欧拉角的计算公式为：

自此，初步得到了加工轨迹点的位置(x,y,z空间坐标)与加工姿态(α，β，γ欧拉角)。

步骤1.5：按照加工次序连接所有生成的轨迹点，并设置轨迹点的运动模式，将第一个轨迹点的运动模式设计为关节空间运动“movj”，其余轨迹点的运动模式设置为笛卡尔空间直线运动“movl”。

步骤2中，将所生成的轨迹点按照位姿和机器人机械臂的运动范围进行运动学分析，找出不可达点、轴超限点及跳变点，具体步骤如下：

步骤2.1：选取需要进行检测的曲面轨迹，提取轨迹信息，主要包括位置信息x、y、z和姿态信息三个欧拉角以及相应的运动方式(“movj”，“movl”)，并按轨迹加工顺序将轨迹点按顺序存储，并获取轨迹点的总个数，设置运动指令计数标志，初始化为0；

步骤2.2：如果计数标志小于轨迹点总个数，获取当前轨迹点，提取位置、姿态、运动方式等轨迹点信息，反之，进入步骤2.6；

步骤2.3：解析运动指令，通过步骤2.2得到轨迹点信息，通过机器人逆解算法得到在该轨迹点下的运动关节角，如果逆解无解或jacobian矩阵无反函数，则该点不可达，或该点为奇异点，将轨迹点设为不可达点或奇异点，运动指令计数标志加一，返回步骤2.2；如果逆解有解，解出六个关节角并保存，到步骤2.4；

步骤2.4：由步骤2.3得知当前轨迹点不是不可达点或奇异点，将逆解求出的六个关节角进行轴超限检测，如果有任意一轴的关节角超出范围，将该轨迹点的轴超限状态设置为激活状态，如果该点是正常点，到步骤2.5；

步骤2.5：根据运动方式，对该点进行插补，在插补过程中，得到一个插补点，如果该点不是最后一个插补点，返回步骤2.2；如果该点是最后一个插补点，将计数标志加1，返回步骤2.2；

步骤2.6：结束。

由上述步骤，可以检测出该复杂曲面轨迹加工过程中，所有遇到的不可达点及轴超限点，并保存了所有轨迹点及其插补点的关节角；还需要进行跳变点检测，为了保护机器人运动过程中的稳定性，不能允许空间中两个相邻很近的点，突然某个关节轴的变化很大，导致实际中机器人无法正常工作。由上述检测不可达点与轴超限点过程中已经将轨迹点和其插补点的关节角都做了保存。

跳变点检测具体步骤如下：

步骤a：首先设置六个关节角的变化量分别为a1，a2，a3，a4，a5，a6；初始化为0，得到所有点的个数(包括轨迹点和插补点)，设置计数器，初始化为0，设置前一步的六个关节角的值pre1～pre6，初始化为第一个点六个关节角的值；

步骤b：通过计数器从所有点中按顺序得到一个轨迹点(注意不是插补点)，得到这个点的六个关节角cur1～cur6；

步骤c：通过公式：得到当前轨迹点关节角的变化量，如果关节角变化量大于50度则将该轨迹点中的跳变标志位激活，表面该轨迹点是一个跳变点；

步骤d：判断计数器是否小于点的总数，是则返回步骤b，反之，结束；

通过以上方法将曲面每个轨迹点的状态数据检测更新，将轨迹点分为五类：正常点、不可达点(或奇异点)、轴超限点、跳变点。

本发明的异常点检测，其功能是对步骤1中所生成的复杂曲面加工轨迹进行异常点检测，其目的是为了排除生成的轨迹中存在机器人在运动学范围内无法正常加工到的轨迹点(不可达点)，或者干扰正常加工顺序的一些异常点(轴超限点和跳变点)。具体方法是将轨迹点逐个进行逆运动学分析，通过其表示的机器人末端位姿来解出其运动的六个关节角，对于满足piper准则的机器人，如果一个轨迹点的逆运动学无解，则表示该点不可达，可以标记为不可达点；对于有解的轨迹点，首先判断解算出来的各个关节角是否满足机器人的运动范围，如果不满足，则标记为轴超限点，满足则进行跳变点的判断，对于所有轨迹点，计算相邻两点间关节角的变化量，如果有任何一个关节角的变化量超出设定阈值，则标记为跳变点，否则，这个点为正常点，可以满足加工需求。

对于运动学逆解，主要采用几何法和欧拉角变换相结合的方法进行，相比较于单纯使用几何法或者代数法，代码量明显减少，可以加快算法的运行速度。

步骤3中，对检测出来的异常点进行位姿调整，实现对于单轨迹点、轨迹段、多轨迹点的多种编辑的功能，参考图10，其主要编辑操作包括轨迹的旋转、复制、平移轴反向、插入拆分等，用来对异常轨迹点进行操作，以求变为正常可以加工的轨迹点，得到可以正常加工并满足工艺的轨迹段；

对于平移和旋转：平移操作是针对单独轨迹点沿着自己的姿态坐标系x、y、z轴进行平移，平移变换可以视为平移算子左乘原坐标系得到平移后新坐标系，矩阵平移算子定义如下：

其中：p、py、pz分别是x、y、z轴方向的平移分量；p为平移向量，且有

旋转操作是对于轨迹点姿态的改变，不改变轨迹点的位置信息，和平移操作一样，要以三个姿态坐标系x、y、z为准进行相应的旋转操作，可以绕某一个轴进行旋转，也可以安顺序依次绕x、y、z轴旋转，相应的旋转矩阵rx(θx)、ry(θy)、rz(θz)分别为：

式中：θx、θy、θz分别是绕x、y、z轴旋转的角度。

对于轨迹点复制和删除：轨迹点复制是创建一个和该轨迹点位姿完全一样的新的轨迹点对象，默认将新生成的轨迹点加到原有轨迹点之前的位置。删除是根据需求将轨迹点列表中不需要的轨迹点删掉并更新轨迹。

x/z轴反向：这个操作是针对单轨迹点的姿态坐标x轴正方向和z轴正方向反向。在x轴反向时，保持z轴方向不变，y轴的方向由新得到的x轴和z轴做向量的叉乘得到；同理，在改变z轴方向时，保证x轴的方向不改变，y轴的方向由新得到的z轴的方向和x叉乘得到。

对于段轨迹的操作，除了上面的对单轨迹点的操作外，还有位姿的统一、隐藏、显示、仿真、轨迹反向、关联零件、上下移动等功能。

本发明的修正是对步骤2检测到的异常点进行编辑纠正，以满足机器人正常加工的要求，因为根据截面线发初步生成的一系列轨迹点可能有一些不满足机器人运动学的要求，需要进行人工的微调。本发明提供对生成的轨迹(一条轨迹由至少一个轨迹点组成)三个层次的操作编辑，分别是：单个轨迹点、任意一个轨迹段、多个连续的轨迹点，其中单轨迹点编辑是针对单独的一个异常点进行纠正，主要操作包括平移、旋转、复制、删除、轴反向等，还可以显示轨迹点的状态及其所有的数据属性；轨迹段编辑是要针对一个完整的轨迹段进行编辑操作，主要操作包括轨迹段的逆序、复制、删除、仿真顺序的上移下移、分割合并、平移及旋转、轴反向及位姿统一。多轨迹点编辑是针对一段轨迹中多个连续轨迹点进行的编辑操作，主要应用场景是轨迹段中有多个连续的异常轨迹点，一个一个编辑纠正效率低下，对于需要进行相同操作的连续轨迹点，可以通过多点选取进行批量操作，节省编程效率。

步骤4中，将自动生成的轨迹可以通过关联零件操作与加工零件相互关联，可以平移和旋转工件，与该工件关联的轨迹也可以随工件进行相应的变换，主要是借助occ中gp_trsf描述模型对象位姿变换，具体的步骤如下：

步骤1.4.1：首先获取当前轨迹所在工件模型的变化gp_trsf；

步骤1.4.2：获取轨迹中所有点的位置坐标，构造对应的gp_pnt，并且得到读取所有点的姿态信息，即三个欧拉角，根据欧拉角构造对应的三个方向gp_dir；

步骤1.4.3：根据步骤1.4.1得到的工件的变换矩阵gp_trsf，将该轨迹中所有点的gp_pnt和三个方向gp_dir变换到相应的位置；

步骤1.4.4：计算该轨迹中所有点变换后的欧拉角(根据每个点的gp_dir)

步骤1.4.5：更新轨迹数据，并同时更新交互界面中的显示模型；

在曲面轨迹自动生成、异常点检测、和通过轨迹编辑操作用户纠正异常点后，要进行复杂曲面加工仿真，通常情况下由于机器人空间位姿的限制，很多复杂曲面无法再一个姿态下加工完成，通常需要两台机器人协助作业完成加工，对于两台机器人异步协同加工的仿真，通常需要单独设置一类特殊的通信轨迹点来通过多线程并发的方式来完成，并通过线程之间的通信来完成机器人之间的通信以完成相互协作的任务，达到异步协同工作，实现双机器人协同加工一个复杂曲面，具体步骤如下：

步骤1.4.6：读取两台机器人所有的轨迹信息，分别存储在相应的轨迹列表中；

步骤1.4.7：分别对两个机器人的轨迹点列表根据位姿信息，运动模式进行运动学解析，得到每个轨迹点与插补点的关节角和变换矩阵；

步骤1.4.8：根据步骤1.4.7得到的变换矩阵和关节角，通过_beginthread函数并发两个线程，并保存两个线程的句柄；同时利用变换矩阵对两台机器人的各个关节的位姿进行更新；

步骤1.4.9：对于一个线程执行如下操作：遍历轨迹点，读取一个轨迹点，判断该轨迹点是否为通信轨迹点，如果是，唤起兄弟线程(另一个机器人仿真线程)，然后挂起自己，当整个轨迹遍历完后，唤醒一次兄弟线程，防止发生锁死。

在步骤1.4.7中通过运动学正逆解的方法，可以由末端轨迹点的位姿及运动模式等信息得到机器人六个关节的变换矩阵，在步骤1.4.9中将得到的变换矩阵依次作用在机器人的occ三维实体上，进行仿真画面的更新，具体在opencascade环境下实现三维模型空间平移和空间旋转的类gp_trsf中，通过得到的旋转矩阵和该类中的setrotation来实现绕轴的旋转运动以及settranslation函数来实现平移运动，或者两个同时使用来实现模型的旋转和平移的叠加效果。

即，在复杂加工环境下，需要机器人的协同来完成加工任务，通过通信轨迹点的异步协同加工，利用多线程并发及线程间的通信来模拟实际协同加工过程中的协同操作工序，并可利用线程交互将机器人运动学正逆解求出的各个关节的变换矩阵通过几何造型平台occ中的矩阵变换接口来不断刷新仿真交互界面中的画面。