CN109664296A - 一种面向机器人砂带打磨的优化轨迹搜索方法 - Google Patents

Abstract

本发明公开了一种面向机器人砂带打磨的优化轨迹搜索方法，包括以下步骤：规划打磨刀路，生成刀位点并建立刀位点坐标系；为打磨平台建立相应坐标系；建立调整数学模型，通过数学模型进行调整，得到调整后工具坐标系与机器人基坐标系的相对位姿计算公式；建立打磨过程坐标平面，建立机器人打磨轨迹的结点模型；对打磨过程坐标平面纵坐标进行离散，用结构体保存结点信息，得到结构体矩阵；搭建打磨仿真场景，并构建优化机器人打磨轨迹搜索函数，对结点结构体矩阵进行搜索，搜索失败，则结束流程；搜索成功，将搜索信息转换成机器人控制柜命令实现自动化抛光；本发明得到的轨迹：无碰撞、关节量变化小，为复杂工件打磨提供有效打磨轨迹生成方法。

Description

一种面向机器人砂带打磨的优化轨迹搜索方法

技术领域

本发明涉及机器人砂带打磨的研究领域，特别涉及一种面向机器人砂带打磨的优化轨迹搜索方法。

背景技术

随着产业的发展，工业产品越来越多地运用了不规则曲面，同时对于曲面的表面形状精度和粗糙度也提出了一定的要求。这些曲面在半精加工和精加工阶段一般都需要经历砂带打磨工序，使曲面获得较为光滑的表面。

但手工打磨的现场环境普遍比较恶劣，打磨作业现场的巨大噪音和金属粉尘对于操作者身体的危害受到越来越多人的关注，使得国内雇用打磨工人的费用快速上涨，所以越来越多的企业开始用自动化机器代替工人来进行打磨作业。相对于雇用工人进行手工打磨的生产方式，利用自动化机器打磨生产具有高效率、高精度以及产品质量稳定等优点。

现阶段，工业机器人主要的编程方式可分为在线示教和离线编程两种。在线示教编程就是操作人员操作机器人使末端的工件在砂带轮上模拟打磨的过程，在模拟打磨的过程中由机器人控制系统实时采集工业机器人各个关节的关节量，之后让机器人沿着之前模拟的轨迹重复运动；一旦曲面的形状变得复杂，需要示教的位姿将大大增加，示教编程不但耗时长而且加工质量往往也不能令人满意。

与在线示教相对应的是离线编程方法，则主要是通过工人操作示教软件，以记录方式或算法生成加工轨迹，实现远离实际加工场地、无需与加工进程同步的机器人或数控机床编程。为了能够快速地生成高质量的机器人砂带打磨轨迹，对离线编程方式生成机器人砂带打磨轨迹的方法进行了研究。由于机器人执行离线编程方法生成的机器人打磨轨迹时，有可能会出现碰撞的情况，如机器人与设备发生碰撞、工件与砂带轮发生碰撞等；同时如果机器人在打磨的过程关节角变化太大的话，关节角可能会出现超限的情况。

很多学者对机器人的无碰撞路径规划作了深入的研究，并提出了许多经典的方法，如C空间法、人工势场法等方法；近年来，人工神经网络、遗传算法、模糊算法等智能方法也被应用于机器人无碰路径规划领域中；但是对于机器人砂带打磨情况的无碰优化和关节角优化的方法尚未见文献报道。

发明内容

本发明的主要目的在于克服现有技术的缺点与不足，提供一种面向机器人砂带打磨的优化轨迹搜索方法。针对复杂形状工件的打磨，首先在工件的被打磨面上规划打磨刀路并生成一系列刀位点；然后搭建机器人砂带打磨的仿真场景，并为仿真场景编写二次开发程序，在二次开发程序中编写优化机器人打磨轨迹搜索函数；搜索函数根据打磨刀路，在仿真场景中搜索无碰优化及关节角优化的机器人打磨轨迹，如果搜索失败，则结束整个流程并输出搜索失败的提示，如果搜索成功，则将搜索得到的机器人打磨轨迹转换成机器人控制柜可执行的代码实现自动化抛光，从而为复杂工件的打磨提供一种更加有效的机器人砂带打磨轨迹生成方法。

本发明的目的通过以下的技术方案实现：

一种面向机器人砂带打磨的优化轨迹搜索方法，包括以下步骤：

S1、在工件的被打磨面上规划打磨刀路并生成一系列刀位点，并在刀位点上建立刀位点坐标系；

S2、为机器人打磨平台上的单元建立坐标系，所述坐标系包含：机器人基坐标系、机器人末端关节坐标系、初始工具坐标系、被打磨曲面刀位点上坐标系、工件坐标系；

S3、建立用于描述工具坐标系位置调整的数学模型，通过描述工具坐标系位置调整的数学模型进行调整，得到调整后工具坐标系与机器人基坐标系的相对位姿计算公式；

S4、建立用于描述机器人打磨过程的坐标平面，并建立机器人打磨轨迹的结点模型，所述结点模型用于生成机器人打磨轨迹；

S5、对描述机器人打磨过程的坐标平面的纵坐标进行离散，并用一个结构体保存一个结点的信息，得到一个结构体矩阵；

S6、搭建机器人砂带打磨仿真场景，并构建优化机器人打磨轨迹搜索函数，对结点结构体矩阵中的元素进行搜索，搜索失败，则结束流程并输出搜索失败提示；搜索成功，则进入下一步；

S7、根据搜索得到的机器人打磨轨迹中结点结构体包含的信息，将其转换成机器人控制柜可执行的代码实现自动化抛光。

进一步地，步骤S1，所述打磨刀路是打磨过程中，砂带轮与工件的被打磨面进行接触的路径；所述刀位点是通过插补算法在打磨刀路上生成的一系列离散的点；所述坐标系是指固定在每一个刀位点上的坐标系；

进一步地，所述固定在每一个刀位点上的坐标系，其坐标系原点为刀位点，其Z轴方向为被打磨曲面在刀位点处指向外部的法向，其X轴方向为刀位点处打磨刀路的切向，其Y轴方向通过右手定则确定；

进一步地，步骤S2中，所述坐标系轴向，具体为：机器人基坐标系是机器人自带的坐标系，即世界坐标系；机器人末端关节坐标系原点位于机器人末端法兰盘中心，其Y轴方向为法兰盘径向方向，其X轴为法兰盘轴向方向，其Z轴则可通过右手定则确定；砂带轮上的初始工具坐标系的原点位置为过砂带轮轴线的水平面与砂带中轴线的交点，其Y轴方向为与水平面垂直向上的方向，其X轴为与砂带轮左侧面垂直向左的方向，其Z轴则可通过右手定则确定；被打磨曲面刀位点上的坐标系指固定在每一个刀位点上的坐标系，其原点位置位于刀位点上，Z轴的方向为被打磨曲面在刀位点处指向外部的法向，X轴的方向为刀位点处打磨刀路的切向，Y轴方向通过右手定则确定；工件坐标系的原点位于工件物理重心，XYZ轴的方向与机器人末端关节坐标系一致；

进一步地，所述步骤S3，具体为：

把砂带轮用于打磨加工区域分为三个部分：左旋装区、右旋转区、平移区；初始工具坐标系{G₀}分别在三个区域内进行相应调整，调整后工具坐标系为{G}，通过描述工具坐标系位置调整的数学模型进行调整，即将位置调整数字化，便于计算和表达，调整过程如下：

W1、在砂带轮的宽度范围内，初始工具坐标系沿砂带轮的宽度作平移调整，设砂带轮的宽度为W，当变量t∈[-w/2，w/2]时，调整后工具坐标系位于平移区，则调整后工具坐标系相对机器人基坐标系的位置为：

W2、将初始工具坐标系{G₀}平移到砂带轮最左端后记为{L}，则{L}位于左旋转区，能绕Y轴作顺时针旋转调整；设旋转最大角度为D，当变量t∈[-w/2-D，-w/2]时，调整后工具坐标系位于左旋转区，绕Y轴顺时针旋转角度为d＝-w/2-t，则调整后工具坐标系相对机器人基坐标系的位置为：

W3、将初始工具坐标系{G₀}平移到砂带轮最右端后记为{R}，则位于右旋转区，能绕Y轴作顺时针旋转调整；设旋转最大角度为D，当变量t∈[w/2，w/2+D]时，调整后工具坐标系位于右旋转区，绕Y轴顺时针旋转角度为d＝t-w/2，则调整后工具坐标系相对机器人基坐标系的位置为：

进一步地，所述步骤S4，具体为：

以工件被打磨面上刀位点的序号为横坐标，以调整后工具坐标系与机器人基坐标系相对位姿计算公式中变量t为纵坐标，建立坐标平面；在打磨第i个刀位点的时候，刀位点坐标系{M_i}与工具坐标系{G}重合，则机器人末端关节坐标系{6}相对于基坐标系{B}的位姿矩阵为：

其中，为机器人末端关节坐标系相对于基坐标系的位姿矩阵；为工件坐标系相对机器人末端关节坐标系的位姿矩阵；为刀位点坐标系相对于工件坐标系的位姿矩阵；为参数t对应的调整后工具坐标系相对于机器人基坐标系的位姿矩阵；

将计算得到的位姿矩阵代入机器人逆运动学求解算法中，则求得机器人打磨刀位点i时的关节空间位姿θ＝[θ₁～θ₆]；

建立机器人打磨轨迹结点模型，具体为：在坐标平面上的每一个点为一个结点，记为Node(I,t)，表示第i个刀位点用参数t所对应的生成机器人打磨姿态；为每一个刀位点指定合适参数t的值，并将刀位点上的结点依照刀位点序号进行排列，组成一条机器人打磨轨迹，即机器人打磨轨迹结点模型；

进一步地，所述步骤S5，具体为：

设定一个正整数M，将描述机器人打磨过程坐标平面的纵坐标离散成M个值，则在坐标平面上得到一个行数为M，列数为N的结点矩阵；用一个结点结构体保存一个结点信息，得到一个M行N列的结点结构体矩阵NodeMatrix；

所述结点结构体的成员变量包含：变量t、变量i、长度为6的数组RobotPose[6]、长度为3的数组Origin[3]、长度为3的数组AxisZ[3]、长度为3的数组AxisX[3]；所述变量t，用于保存该结点对应的纵坐标的值；所述变量i，用于保存该结点对应刀位点的序号，即横坐标；所述长度为6的数组RobotPose[6]，用于保存刀位点i根据工具坐标系位姿矩阵所生成的机器人关节空间姿态；所述长度为3的数组Origin[3]，用于保存刀位点坐标系{M_i}的原点位置；所述长度为3的数组AxisZ[3]，用于保存{M_i}的Z轴的方向向量；所述长度为3的数组AxisX[3]，用于保存{M_i}的X轴的方向向量；

进一步地，所述步骤S6，具体为：

利用开源机器人仿真与集成开发平台搭建机器人砂带打磨仿真场景，通过VisualStudio2013与Qt构建优化机器人打磨轨迹搜索函数；优化机器人打磨轨迹搜索函数对结点结构体矩阵NodeMatrix中的元素进行搜索，并建立一个空的结点结构体列表RobotTrackList用来保存搜索的结果。

所述优化机器人打磨轨迹搜索函数，步骤如下：

Y1、判断轨迹列表是否为空，判断结果为True，则将结点结构体矩阵第1列中与机器人打磨前的初始位姿最接近的结点作为Node，并将第一个刀位点所对应的结点结构体Node1插入轨迹列表RobotTrackList中；判断结果为False，则进入下一步；

Y2、提取轨迹列表RobotTrackList中最后一个结点结构体的刀位点序号i，判断i是否等于N，判断结果为True，则完成轨迹搜索，结束函数并返回函数调用点；判断结果为False，则进入下一步；

Y3、遍历NodeMatrix矩阵第i+1列中所有结点，求解出分别将这些结点结构体作为Node i+1时，各结点对应的关节量变化总量Δθ；对NodeMatrix矩阵第i+1列结点按Δθ值从小到大进行排序，新建一个变量K，令K＝1；

Y4、搜索NodeMatrix矩阵第i+1列的第K个结点，将该结点作为Node i+1，对Node i+1的位姿进行碰撞检测；没有发生碰撞，则进入步骤Y5；发生碰撞，则进入步骤Y6；

Y5、递归调用优化机器人打磨轨迹搜索函数，进行下一轮搜索，当递归调用的优化机器人打磨轨迹搜索函数结束并返回该位置，提取轨迹列表RobotTrackList中最后一个结点结构体的刀位点序号i，判断i是否等于N；当i等于N时，完成轨迹搜索，结束函数并返回函数调用点；当i不等于N时，进入下一步；

Y6、判断K是否等于M，当K不等于M，弹出轨迹列表RobotTrackList中Node i+1结点，令K＝K+1，并返回步骤Y4，继续执行；当K等于M，说明结点Node i不能在NodeMatrix矩阵的第i+1列结点中搜索到合适结点，弹出轨迹列表RobotTrackList中Node i、Node i+1两个结点，结束函数此层递归调用，返回函数上一层递归调用点；

在优化机器人打磨轨迹搜索函数运行结束后，判断搜索是否成功；提取轨迹列表RobotTrackList中最后一个结点所对应的刀位点序号i，判断i是否等于N；如果i≠N，说明没有顺利完成搜索，输出“优化失败”并清空轨迹列表RobotTrackList；如果i＝N，则输出“优化成功”和轨迹列表RobotTrackList；

进一步地，所述搜索失败，具体为：在优化机器人打磨轨迹搜索函数运行结束后，提取轨迹列表中最后一个结点所对应的刀位点序号i，i不等于刀位点总数，没有完成搜索，输出优化失败并清空轨迹列表；所述搜索成功，具体为：在优化机器人打磨轨迹搜索函数运行结束后，提取轨迹列表中最后一个结点所对应的刀位点序号i，i等于刀位点总数，完成搜索，输出优化成功和轨迹列表。

本发明与现有技术相比，具有如下优点和有益效果：

1、本发明使用户在仿真的环境中完成工件的示教操作，能够使工人能够从恶劣、危险的机加工环境中解放出来；

2、本发明最终得到的机器人轨迹具有无碰撞、关节量变化小的优点，从而为复杂工件的打磨提供一种更加有效的机器人砂带打磨轨迹生成方法。

附图说明

图1是本发明所述一种面向机器人砂带打磨的优化轨迹搜索方法的方法流程图；

图2是本发明所述实施例中打磨工件被打磨面上的刀位点坐标系示意图；

图3是本发明所述实施例中机器人打磨平台建立的坐标系示意图；

图4是本发明所述实施例中在砂带轮上的初始工具坐标系示意图；

图5是本发明所述实施例中砂带轮加工区域划分示意图；

图6是本发明所述实施例中初始工具坐标系的正向平移调整示意图；

图7是本发明所述实施例中初始工具坐标系的负向平移调整示意图；

图8是本发明所述实施例中初始工具坐标系的顺时针旋转调整示意图；

图9是本发明所述实施例中初始工具坐标系的逆时针旋转调整示意图；

图10是本发明所述实施例中机器人打磨过程中发生碰撞情况下的坐标平面图；

图11是本发明所述实施例中机器人打磨过程中无碰撞情况下的坐标平面图；

图12是本发明所述实施例中坐标平面纵坐标进行离散后的结点矩阵示意图；

图13是本发明所述实施例中结点结构体矩阵示意图；

图14是本发明所述实施例中二次开发软件架构图。

图中，1-打磨机，2-砂带轮。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例：

图1是本发明一种面向机器人砂带打磨的无碰优化及关节角优化轨迹搜索方法的流程图，如图所示，包括以下步骤：

步骤1：在工件的被打磨面上规划打磨刀路并生成一系列刀位点，刀位点的总数记为N。并在每一个刀位点上建立坐标系。

在工件的被打磨面上规划打磨刀路，并通过插补算法在打磨刀路上生成一系列刀位点，如图2所示，刀位点的总数记为N。在每一个刀位点上建立坐标系{M_i}，其中i为刀位点的序号，Z轴的方向为曲面指向外部的法向，X轴的方向为刀位点处打磨刀路的切向，Y轴方向通过右手定则确定。由于刀位点的空间坐标、曲面的法向量以及打磨刀路的切向量都是通过工件自身坐标系{T}进行描述的。根据位姿矩阵的定义，可以得到坐标系{M_i}相对于工件自身坐标系{T}的位姿矩阵

步骤2：为机器人打磨平台上的单元建立坐标系。

在机器人打磨平台上建立如图3所示的坐标系。其中，{B}是机器人基坐标系，{6}是机器人末端关节的坐标系，{G₀}是在砂带轮上的初始工具坐标系，{M_i}是被打磨曲面上第i个刀位点上的坐标系，{T}是工件坐标系。坐标系{T}与坐标系{6}的相对位姿由工件装夹夹具的结构确定。初始工具坐标系{G₀}在砂带轮上的位置如图4所示，砂带轮的轴线、基坐标系B的X_BY_B平面均与水平面相平行；过砂带轮轴线作一水平面，将水平面与砂带中轴线的交点，作为G₀坐标系的原点位置；将与水平面垂直向上的方向，作为G₀坐标系的Y轴方向；与砂带轮左侧面垂直向左的方向，作为G₀坐标系的X轴；最后通过右手定则确定G₀坐标系的Z轴；初始工具坐标系{G₀}与基坐标系{B}的相对位姿则通过标定获得。

坐标系轴向，具体为：机器人基坐标系是机器人自带的坐标系，即世界坐标系；机器人末端关节坐标系原点位于机器人末端法兰盘中心，其Y轴方向为法兰盘径向方向，其X轴为法兰盘轴向方向，其Z轴则可通过右手定则确定；砂带轮上的初始工具坐标系的原点位置为过砂带轮轴线的水平面与砂带中轴线的交点，其Y轴方向为与水平面垂直向上的方向，其X轴为与砂带轮左侧面垂直向左的方向，其Z轴则可通过右手定则确定；被打磨曲面刀位点上的坐标系指固定在每一个刀位点上的坐标系，其原点位置位于刀位点上，Z轴的方向为被打磨曲面在刀位点处指向外部的法向，X轴的方向为刀位点处打磨刀路的切向，Y轴方向通过右手定则确定；工件坐标系的原点位于工件物理重心，XYZ轴的方向与机器人末端关节坐标系一致。

步骤3：建立用于描述工具坐标系位置调整的数学模型，得到调整后工具坐标系与机器人基坐标系的相对位姿的计算公式。

把砂带轮用于打磨加工的区域分成3个部分，分别为左旋转区，右旋转区以及平移区，如图5所示。初始工具坐标系{G₀}可分别在这三个区域内进行相应的调整，调整后的工具坐标系位置记为{G}。初始工具坐标系调整的方法以及调整后工具坐标系{G}与基坐标系{B}相对位姿的计算公式如下：

(1)在砂带轮的宽度范围内，初始工具坐标系{G₀}可以沿着砂带轮的宽度作平移调整。设砂带轮的宽度为w，当变量t∈[-w/2，w/2]时，工具坐标系{G}位于平移区，如图6和图7所示，此时工具坐标系{G}相对于基坐标系{B}的位置可通过以下公式获得：

(2)将{G₀}坐标系平移到砂带轮的最左端后记为{L}，此时{L}位于砂带轮的左旋转区范围内，{L}可以绕Y轴作顺时针旋转调整。设坐标系旋转的最大角度为D度，当变量t∈[-w/2-D，-w/2]时，工具坐标系{G}位于左旋转区，绕坐标系的Y轴顺时针旋转的角度为d＝-w/2-t，如图8所示，此时工具坐标系{G}相对于基坐标系{B}的位置可通过以下公式获得：，

(3)将{G₀}坐标系平移到砂带轮的最右端后记为{R}，此时{R}位于砂带轮的右旋转区范围内，{R}可以绕Y轴作逆时针旋转调整。设坐标系旋转的最大角度为D度，当变量t∈[w/2，w/2+D]时，工具坐标系{G}位于右旋转区，绕坐标系的Y轴顺时针旋转的角度为d＝t-w/2，如图9所示，此时工具坐标系{G}相对于基坐标系{B}的位置可通过以下公式获得：

综合上述可知，只要给定变量t的具体数值，根据调整后工具坐标系{G}的计算公式便可以得到一个位姿矩阵即每一个t的值都对应一个工具坐标系{G}的位置。

步骤4：以刀位点的序号为横坐标，以工具坐标系与基坐标系相对位姿计算公式中的变量为纵坐标，建立用于描述机器人打磨过程的坐标平面，坐标平面上的每一个点称为结点。

建立机器人打磨轨迹的结点模型。

以工件被打磨面上刀位点的序号为横坐标，以工具坐标系{G}与基坐标系{B}相对位姿计算公式中的变量t为纵坐标，可建立如图10和图11所示的坐标平面。坐标系平面中的阴影部分表示会产生碰撞的区域。

机器人夹持工件进行打磨实质上就是工件被打磨面上刀位点依次与砂带轮进行接触的过程。在打磨第i个刀位点的时候，坐标系{M_i}与工具坐标系{G}重合，此时机器人末端关节坐标系{6}相对于基坐标系{B}的位姿矩阵可通过下面公式计算得到：

其中，为机器人末端关节坐标系{6}相对于基坐标系{B}的位姿矩阵；为工件自身坐标系{T}相对于坐标系{6}的位姿矩阵；为坐标系{M_i}相对于工件自身坐标系{T}的位姿矩阵；为参数t所对应的工具坐标系{G}相对于基坐标系{B}的位姿矩阵。将计算得到的位姿矩阵代入机器人的逆运动学求解算法中，可求得机器人打磨刀位点i时的关节空间位姿θ＝[θ₁～θ₆]。

建立机器人打磨轨迹的结点模型。在如图10和图11的坐标平面中，坐标平面上的每一个点(i,t)称为一个结点，记为Node(i,t)，表示第i个刀位点用参数t所对应的生成机器人打磨姿态。由于一个刀位点不可能在同一时刻用砂带轮的不同位置进行打磨，因此每一个刀位点上只有一个结点。为每一个刀位点指定一个合适的参数t的值，并将刀位点上的结点依照刀位点的序号进行排列，便可以组成一条机器人打磨轨迹。在生成机器人打磨轨迹时，应该避免轨迹上的结点落在阴影部分。

步骤5：设定一个正整数值M，将描述机器人打磨过程的坐标平面的纵坐标离散成M个值，在坐标平面上可得到一个行数为M、列数为刀位点数N的结点矩阵。用一个结构体来保存一个结点的信息，可得到一个M行N列的结构体矩阵。

一条机器人打磨轨迹是由一系列结点所组成的列表。打磨轨迹的优化过程实质上就是在包含所有结点的集合中，搜索出一条无碰撞且关节变化量较小的机器人打磨轨迹。由于t是一个连续变量，因此坐标系的每一列都有无数个结点。可以设定一个正整数值M，把变量t均匀离散成M个值，得到如图12所示的M行N列的结点矩阵，其中N为刀位点的总数。

建立一个结构体来保存一个结点的信息。结点结构体的成员变量有：(1)变量t，用于保存该结点对应的纵坐标的值；(2)变量i，用于保存该结点对应刀位点的序号(横坐标)；(3)一个长度为6的数组RobotPose[6]，用于保存刀位点i根据工具坐标系位姿矩阵所生成的机器人关节空间姿态；(4)一个长度为3的数组Origin[3]，用于保存刀位点坐标系{M_i}的原点位置；(5)一个长度为3的数组AxisZ[3]，用于保存{M_i}的Z轴的方向向量；(6)一个长度为3的数组AxisX[3]，用于保存{M_i}的X轴的方向向量。

为图10中的每一个结点建立一个结点结构体，并将每一个结点对应的信息赋给结点结构体的成员变量，可得到一个M行N列的结点结构体矩阵NodeMatrix，如图13所示。

步骤6：搭建机器人砂带打磨的仿真场景，并为仿真场景编写二次开发程序。在二次开发程序中编写优化机器人打磨轨迹搜索函数，对结点结构体矩阵中的元素进行搜索。如果搜索失败，则结束整个流程并输出搜索失败的提示。如果搜索成功，则执行步骤7。

步骤7：首先，利用开源机器人仿真与集成开发平台V-REP搭建用于机器人砂带打磨的仿真场景，其中包括砂带机、被打磨工件和六自由度机器人。

然后，采用Visual Studio2013与Qt为V-Rep编写了一个二次开发程序，该程序的架构如图14所示。二次开发程序中包含优化机器人打磨轨迹搜索函数，而V-Rep软件中则运行场景仿真、碰撞检测与运动仿真三个功能，二次开发程序与V-Rep软件之间的通信是通过调用V-Rep的远程接口RemoteAPI来实现的，二次开发程序可以将解算及优化后的机器人关节空间位姿传输到V-Rep中进行仿真，V-Rep则可以将仿真结果与碰撞检测结果传输给二次开发程序中。该程序根据返回的碰撞检测结果，进行无碰优化及关节角优化轨迹的搜索。

优化机器人打磨轨迹搜索函数对结点结构体矩阵NodeMatrix中的元素进行搜索，并建立一个空的结点结构体列表RobotTrackList用来保存搜索的结果。其中，“优化机器人打磨轨迹搜索函数”的步骤如下：

(1)判断轨迹列表是否为空；如果结果为True，则将结点结构体矩阵第1列中与机器人打磨前的初始位姿最接近的结点作为Node₁，并将Node₁插入轨迹列表RobotTrackList中；如果结果为False，则执行步骤(2)；

其中，Node₁表示第1个刀位点所对应的结点结构体。

(2)提取轨迹列表RobotTrackList中最后一个结点结构体中的刀位点序号i，判断i是否等于N；如果结果为True，说明已经顺利完成轨迹搜索，结束函数并返回到函数调用点；如果结果为False，则执行步骤(3)。

(3)遍历NodeMatrix矩阵第i+1列中所有结点，求解出分别将这些结点结构体作为Node_i+1时，各结点对应的关节量变化总量Δθ。对NodeMatrix矩阵的第i+1列结点按Δθ的值从小到大重新进行排序。新建一个变量k，并令k＝1。

(4)搜索NodeMatrix矩阵第i+1列的第k个结点，将该结点作为Node_i+1，对Node_i+1的位姿进行碰撞检测。

如果没有发生碰撞，则执行第(5)步。

如果发生碰撞，则跳至第(6)步。

(5)递归调用“优化机器人打磨轨迹搜索函数”，进行下一轮的搜索。当递归调用的函数结束并返回该位置时，提取轨迹列表RobotTrackList中最后一个结点所对应的刀位点序号i，判断i是否等于N。

如果i等于N，说明已经顺利完成轨迹搜索，结束函数并返回到函数调用点。

如果i不等于N，则执行步骤(6)。

(6)判断k＝M是否成立。

如果k不等于M，弹出轨迹列表RobotTrackList中的Node_i+1结点，令k＝k+1，并返回步骤(4)继续执行该函数。

如果k等于M，说明结点Node_i不能在NodeMatrix矩阵的第i+1列结点中搜索到合适的结点，弹出轨迹列表RobotTrackList中的Node_i、Node_i+1这两个结点。结束函数此层递归调用，返回到函数的上一层递归调用点。

最后，在“优化机器人打磨轨迹搜索函数”运行结束后，判断搜索是否成功。提取轨迹列表RobotTrackList中最后一个结点所对应的刀位点序号i，判断i是否等于N；如果i≠N，说明RecurseSearch()没有顺利完成搜索，输出“优化失败”并清空轨迹列表RobotTrackList；如果i＝N，则输出“优化成功”和轨迹列表RobotTrackList，执行步骤7。

步骤7：根据搜索得到的机器人打磨轨迹中结点结构体包含的信息，转换成机器人控制柜可执行的代码实现自动化抛光。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (9)

1.一种面向机器人砂带打磨的优化轨迹搜索方法，其特征在于，包括以下步骤：

S1、在工件的被打磨面上规划打磨刀路并生成一系列刀位点，并在刀位点上建立刀位点坐标系；

S2、为机器人打磨平台上的单元建立坐标系，所述坐标系包含：机器人基坐标系、机器人末端关节坐标系、初始工具坐标系、被打磨曲面刀位点上坐标系、工件坐标系；

S3、建立用于描述工具坐标系位置调整的数学模型，通过描述工具坐标系位置调整的数学模型进行调整，得到调整后工具坐标系与机器人基坐标系的相对位姿计算公式；

S4、建立用于描述机器人打磨过程的坐标平面，并建立机器人打磨轨迹的结点模型，所述结点模型用于生成机器人打磨轨迹；

S5、对描述机器人打磨过程的坐标平面的纵坐标进行离散，并用一个结构体保存一个结点的信息，得到一个结构体矩阵；

S6、搭建机器人砂带打磨仿真场景，并构建优化机器人打磨轨迹搜索函数，对结点结构体矩阵中的元素进行搜索，搜索失败，则结束流程并输出搜索失败提示；搜索成功，则进入下一步；

S7、根据搜索得到的机器人打磨轨迹中结点结构体包含的信息，将其转换成机器人控制柜可执行的代码实现自动化抛光。

2.根据权利要求1所述的一种面向机器人砂带打磨的优化轨迹搜索方法，其特征在于，步骤S1，所述打磨刀路是打磨过程中，砂带轮与工件的被打磨面进行接触的路径；所述刀位点是通过插补算法在打磨刀路上生成的一系列离散的点；所述坐标系是指固定在每一个刀位点上的坐标系。

3.根据权利要求2所述的一种面向机器人砂带打磨的优化轨迹搜索方法，其特征在于，所述固定在每一个刀位点上的坐标系，其坐标系原点为刀位点，其Z轴方向为被打磨曲面在刀位点处指向外部的法向，其X轴方向为刀位点处打磨刀路的切向，其Y轴方向通过右手定则确定。

4.根据权利要求1所述的一种面向机器人砂带打磨的优化轨迹搜索方法，其特征在于，步骤S2中，所述坐标系轴向，具体为：机器人基坐标系是机器人自带的坐标系，即世界坐标系；机器人末端关节坐标系原点位于机器人末端法兰盘中心，其Y轴方向为法兰盘径向方向，其X轴为法兰盘轴向方向，其Z轴则可通过右手定则确定；砂带轮上的初始工具坐标系的原点位置为过砂带轮轴线的水平面与砂带中轴线的交点，其Y轴方向为与水平面垂直向上的方向，其X轴为与砂带轮左侧面垂直向左的方向，其Z轴则可通过右手定则确定；被打磨曲面刀位点上的坐标系指固定在每一个刀位点上的坐标系，其原点位置位于刀位点上，Z轴的方向为被打磨曲面在刀位点处指向外部的法向，X轴的方向为刀位点处打磨刀路的切向，Y轴方向通过右手定则确定；工件坐标系的原点位于工件物理重心，XYZ轴的方向与机器人末端关节坐标系一致。

5.根据权利要求1所述的一种面向机器人砂带打磨的优化轨迹搜索方法，其特征在于，所述步骤S3具体为：

把砂带轮用于打磨加工区域分为三个部分：左旋装区、右旋转区、平移区；初始工具坐标系{G₀}分别在三个区域内进行相应调整，调整后工具坐标系为{G}，通过描述工具坐标系位置调整的数学模型进行调整，调整过程如下：

W1、在砂带轮的宽度范围内，初始工具坐标系沿砂带轮的宽度作平移调整，设砂带轮的宽度为W，当变量t∈[-w/2，w/2]时，调整后工具坐标系位于平移区，则调整后工具坐标系相对机器人基坐标系的位置为：

W2、将初始工具坐标系{G₀}平移到砂带轮最左端后记为{L}，则{L}位于左旋转区，能绕Y轴作顺时针旋转调整；设旋转最大角度为D，当变量t∈[-w/2-D，-w/2]时，调整后工具坐标系位于左旋转区，绕Y轴顺时针旋转角度为d＝-w/2-t，则调整后工具坐标系相对机器人基坐标系的位置为：

W3、将初始工具坐标系{G₀}平移到砂带轮最右端后记为{R}，则位于右旋转区，能绕Y轴作顺时针旋转调整；设旋转最大角度为D，当变量t∈[w/2，w/2+D]时，调整后工具坐标系位于右旋转区，绕Y轴顺时针旋转角度为d＝t-w/2，则调整后工具坐标系相对机器人基坐标系的位置为：

6.根据权利要求1所述的一种面向机器人砂带打磨的优化轨迹搜索方法，其特征在于，所述步骤S4，具体为：

以工件被打磨面上刀位点的序号为横坐标，以调整后工具坐标系与机器人基坐标系相对位姿计算公式中变量t为纵坐标，建立坐标平面；在打磨第i个刀位点的时候，刀位点坐标系{M_i}与工具坐标系{G}重合，则机器人末端关节坐标系{6}相对于基坐标系{B}的位姿矩阵为：

其中，为机器人末端关节坐标系相对于基坐标系的位姿矩阵；为工件坐标系相对机器人末端关节坐标系的位姿矩阵；为刀位点坐标系相对于工件坐标系的位姿矩阵；为参数t对应的调整后工具坐标系相对于机器人基坐标系的位姿矩阵；

将计算得到的位姿矩阵代入机器人逆运动学求解算法中，则求得机器人打磨刀位点i时的关节空间位姿θ＝[θ₁～θ₆]；

建立机器人打磨轨迹结点模型，具体为：在坐标平面上的每一个点为一个结点，记为Node(I,t)，表示第i个刀位点用参数t所对应的生成机器人打磨姿态；为每一个刀位点指定合适参数t的值，并将刀位点上的结点依照刀位点序号进行排列，组成一条机器人打磨轨迹，即机器人打磨轨迹结点模型。

7.根据权利要求1所述的一种面向机器人砂带打磨的优化轨迹搜索方法，其特征在于，所述步骤S5，具体为：

设定一个正整数M，将描述机器人打磨过程坐标平面的纵坐标离散成M个值，则在坐标平面上得到一个行数为M，列数为N的结点矩阵；用一个结点结构体保存一个结点信息，得到一个M行N列的结点结构体矩阵NodeMatrix；

所述结点结构体的成员变量包含：变量t、变量i、长度为6的数组RobotPose[6]、长度为3的数组Origin[3]、长度为3的数组AxisZ[3]、长度为3的数组AxisX[3]；所述变量t，用于保存该结点对应的纵坐标的值；所述变量i，用于保存该结点对应刀位点的序号，即横坐标；所述长度为6的数组RobotPose[6]，用于保存刀位点i根据工具坐标系位姿矩阵所生成的机器人关节空间姿态；所述长度为3的数组Origin[3]，用于保存刀位点坐标系{M_i}的原点位置；所述长度为3的数组AxisZ[3]，用于保存{M_i}的Z轴的方向向量；所述长度为3的数组AxisX[3]，用于保存{M_i}的X轴的方向向量。

8.根据权利要求7所述的一种面向机器人砂带打磨的优化轨迹搜索方法，其特征在于，所述步骤S6，具体为：

利用开源机器人仿真与集成开发平台搭建机器人砂带打磨仿真场景，通过VisualStudio2013与Qt构建优化机器人打磨轨迹搜索函数；优化机器人打磨轨迹搜索函数对结点结构体矩阵NodeMatrix中的元素进行搜索，并建立一个空的结点结构体列表RobotTrackList用来保存搜索的结果；

所述优化机器人打磨轨迹搜索函数，步骤如下：

Y1、判断轨迹列表是否为空，判断结果为True，则将结点结构体矩阵第1列中与机器人打磨前的初始位姿最接近的结点作为Node，并将第一个刀位点所对应的结点结构体Node1插入轨迹列表RobotTrackList中；判断结果为False，则进入下一步；

Y2、提取轨迹列表RobotTrackList中最后一个结点结构体的刀位点序号i，判断i是否等于N，判断结果为True，则完成轨迹搜索，结束函数并返回函数调用点；判断结果为False，则进入下一步；

Y3、遍历NodeMatrix矩阵第i+1列中所有结点，求解出分别将这些结点结构体作为Nodei+1时，各结点对应的关节量变化总量Δθ；对NodeMatrix矩阵第i+1列结点按Δθ值从小到大进行排序，新建一个变量K，令K＝1；

Y4、搜索NodeMatrix矩阵第i+1列的第K个结点，将该结点作为Node i+1，对Node i+1的位姿进行碰撞检测；没有发生碰撞，则进入步骤Y5；发生碰撞，则进入步骤Y6；

Y5、递归调用优化机器人打磨轨迹搜索函数，进行下一轮搜索，当递归调用的优化机器人打磨轨迹搜索函数结束并返回该位置，提取轨迹列表RobotTrackList中最后一个结点结构体的刀位点序号i，判断i是否等于N；当i等于N时，完成轨迹搜索，结束函数并返回函数调用点；当i不等于N时，进入下一步；

Y6、判断K是否等于M，当K不等于M，弹出轨迹列表RobotTrackList中Node i+1结点，令K＝K+1，并返回步骤Y4，继续执行；当K等于M，说明结点Node i不能在NodeMatrix矩阵的第i+1列结点中搜索到合适结点，弹出轨迹列表RobotTrackList中Node i、Node i+1两个结点，结束函数此层递归调用，返回函数上一层递归调用点；

在优化机器人打磨轨迹搜索函数运行结束后，判断搜索是否成功；提取轨迹列表RobotTrackList中最后一个结点所对应的刀位点序号i，判断i是否等于N；如果i≠N，说明没有顺利完成搜索，输出“优化失败”并清空轨迹列表RobotTrackList；如果i＝N，则输出“优化成功”和轨迹列表RobotTrackList。

9.根据权利要求8所述的一种面向机器人砂带打磨的优化轨迹搜索方法，其特征在于，所述搜索失败，具体为：在优化机器人打磨轨迹搜索函数运行结束后，提取轨迹列表中最后一个结点所对应的刀位点序号i，i不等于刀位点总数，没有完成搜索，输出优化失败并清空轨迹列表；所述搜索成功，具体为：在优化机器人打磨轨迹搜索函数运行结束后，提取轨迹列表中最后一个结点所对应的刀位点序号i，i等于刀位点总数，完成搜索，输出优化成功和轨迹列表。